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

/**
 * 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;
}
 

static Matrix4d combineTransformation(Matrix4d matrix, Vector3d translation, Vector3d rotation) {
    Matrix4d gM = new Matrix4d(matrix);
    Matrix4d m = new Matrix4d();

    m.setIdentity();
    m.setTranslation(translation);
    gM.mul(m);

    m.setIdentity();
    m.rotZ(rotation.z);
    gM.mul(m);

    m.setIdentity();
    m.rotY(rotation.y);
    gM.mul(m);

    m.setIdentity();
    m.rotX(rotation.x);
    gM.mul(m);

    return gM;
}
 

public static Matrix4d getMatrixFromAlgebraic(String transfAlgebraic) {
	String[] parts = transfAlgebraic.toUpperCase().split(",");
	double[] xCoef = convertAlgebraicStrToCoefficients(parts[0].trim());
	double[] yCoef = convertAlgebraicStrToCoefficients(parts[1].trim());
	double[] zCoef = convertAlgebraicStrToCoefficients(parts[2].trim());

	Matrix4d mat = new Matrix4d();
	mat.setIdentity();
	mat.setRotation(new Matrix3d(xCoef[0],xCoef[1],xCoef[2],yCoef[0],yCoef[1],yCoef[2],zCoef[0],zCoef[1],zCoef[2]));
	mat.setTranslation(new Vector3d(xCoef[3],yCoef[3],zCoef[3]));
	return mat;
	//return new Matrix4d(xCoef[0],xCoef[1],xCoef[2],xCoef[3],
	//					yCoef[0],yCoef[1],yCoef[2],yCoef[3],
	//					zCoef[0],zCoef[1],zCoef[2],zCoef[3],
	//					0,0,0,1);
}
 

/**
 * 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);
}
 

/**
 * Render the white and grey circles around the exterior, always facing the camera.
 * @param gl2
 */
private void renderOutsideCircle(GL2 gl2) {
	final double whiteRadius=1.05;
	final double greyRadius=1.0;
	final int quality=40;
	
	RobotOverlord ro = (RobotOverlord)getRoot();
	PoseEntity camera = ro.viewport.getAttachedTo();
	ro.viewport.renderChosenProjection(gl2);
	Matrix4d lookAt = camera.getPoseWorld();
	lookAt.setTranslation(MatrixHelper.getPosition(subject.getPoseWorld()));

	gl2.glPushMatrix();

		MatrixHelper.applyMatrix(gl2, lookAt);
		gl2.glScaled(ballSize.get(),ballSize.get(),ballSize.get());
		
		//white circle on the xy plane of the camera pose, as the subject position
		gl2.glColor4d(1,1,1,0.7);
		PrimitiveSolids.drawCircleXY(gl2, whiteRadius, quality);

		//grey circle on the xy plane of the camera pose, as the subject position
		gl2.glColor4d(0.5,0.5,0.5,0.7);
		PrimitiveSolids.drawCircleXY(gl2, greyRadius, quality);

	gl2.glPopMatrix();
}
 

/**
 * 
 * @param mStart matrix of start pose
 * @param mEnd matrix of end pose
 * @param dt time scale, seconds
 * @param cartesianForce 6 doubles that will be filled with the XYZ translation and UVW rotation.
 * @return true if successful
 */
protected boolean getCartesianForceBetweenTwoPoses(Matrix4d mStart,Matrix4d mEnd,double dt,double[] cartesianForce) {
	Vector3d p0 = new Vector3d();
	Vector3d p1 = new Vector3d();
	Vector3d dp = new Vector3d();
	mStart.get(p0);
	mEnd.get(p1);
	dp.sub(p1,p0);
	dp.scale(1.0/dt);

	mStart.setTranslation(new Vector3d(0,0,0));
	mEnd.setTranslation(new Vector3d(0,0,0));
	// get the rotation force
	Quat4d q0 = new Quat4d();
	Quat4d q1 = new Quat4d();
	Quat4d dq = new Quat4d();
	q0.set(mStart);
	q1.set(mEnd);
	dq.sub(q1,q0);
	dq.scale(2/dt);
	Quat4d w = new Quat4d();
	w.mulInverse(dq,q0);
	
	cartesianForce[0]=dp.x;
	cartesianForce[1]=dp.y;
	cartesianForce[2]=dp.z;
	cartesianForce[3]=-w.x;
	cartesianForce[4]=-w.y;
	cartesianForce[5]=-w.z;
	
	return true;
}
 

/**
 * 
 * @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);
}
 

/**
 * Superposition coords2 onto coords1 -- in other words, coords2 is rotated,
 * coords1 is held fixed
 */
private void calcTransformation() {

	// transformation.set(rotmat,new Vector3d(0,0,0), 1);
	transformation.set(rotmat);
	// long t2 = System.nanoTime();
	// System.out.println("create transformation: " + (t2-t1));
	// System.out.println("m3d -> m4d");
	// System.out.println(transformation);

	// combine with x -> origin translation
	Matrix4d trans = new Matrix4d();
	trans.setIdentity();
	trans.setTranslation(new Vector3d(xtrans));
	transformation.mul(transformation, trans);
	// System.out.println("setting xtrans");
	// System.out.println(transformation);

	// combine with origin -> y translation
	ytrans.negate();
	Matrix4d transInverse = new Matrix4d();
	transInverse.setIdentity();
	transInverse.setTranslation(new Vector3d(ytrans));
	transformation.mul(transInverse, transformation);
	// System.out.println("setting ytrans");
	// System.out.println(transformation);
}
 

/**
 * 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);

}
 

/**
 * 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);
	}

}
 

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

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

public void setPosition(Vector3d pos) {
	Matrix4d m = new Matrix4d(pose);
	m.setTranslation(pos);
	setPose(m);
}
 

/**
 * Adds translational component to rotation matrix
 * @param rotTrans
 * @param rotation
 * @return
 */
private void combineWithTranslation(Matrix4d rotation) {
	rotation.setTranslation(centroid);
	rotation.mul(rotation, centroidInverse);
}
 

/**
 * Adds translational component to rotation matrix
 * @param rotation
 * @return
 */
private void combineWithTranslation(Matrix4d rotation) {
	rotation.setTranslation(centroid);
	rotation.mul(rotation, centroidInverse);
}
 

/**
 * Adds translational component to rotation matrix
 * @param rotation
 * @return
 */
private void combineWithTranslation(Matrix4d rotation) {
	rotation.setTranslation(centroid);
	rotation.mul(rotation, centroidInverse);
}