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

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

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

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

public double distanceToTarget() {
	Matrix4d currentMatrix = endEffector.getPoseWorld();
	
	// linear difference in centers
	Vector3d c0 = new Vector3d();
	Vector3d c1 = new Vector3d();
	currentMatrix.get(c0);
	targetMatrix.get(c1);
	c1.sub(c0);
	double dC = c1.lengthSquared();
	
	// linear difference in X handles
	Vector3d x0 = MatrixHelper.getXAxis(targetMatrix);
	Vector3d x1 = MatrixHelper.getXAxis(currentMatrix);
	x1.scale(CORRECTIVE_FACTOR);
	x0.scale(CORRECTIVE_FACTOR);
	x1.sub(x0);
	double dX = x1.lengthSquared();
	
	// linear difference in Y handles
	Vector3d y0 = MatrixHelper.getYAxis(targetMatrix);
	Vector3d y1 = MatrixHelper.getYAxis(currentMatrix);
	y1.scale(CORRECTIVE_FACTOR);
	y0.scale(CORRECTIVE_FACTOR);
	y1.sub(y0);
	double dY = y1.lengthSquared();		

    // now sum these to get the error term.
	return dC+dX+dY;
}
 

@Test
public void testQuatFail() {
	Matrix4d A = new Matrix4d(
			6.123233995736766E-17, 6.123233995736766E-17, 1.0, 44.5,
			-6.123233995736766E-17, -1.0, 6.123233995736766E-17, 1.3949339365687892E-16,
			1.0, -6.123233995736766E-17, -6.123233995736766E-17, 61.967099999999995,
			0.0, 0.0, 0.0, 1.0);
	Quat4d B= new Quat4d();
	A.get(B);
	assert(Double.isNaN(B.x));
	assert(Double.isNaN(B.y));
	assert(Double.isNaN(B.z));
	assert(Double.isNaN(B.w));
}
 

/**
 * Convenience method to call matrixToEuler() with only the rotational component.
 * Assumes the rotational component is a valid rotation matrix.
 * @param mat
 * @return a valid Euler solution to the matrix.
 */
public static Vector3d matrixToEuler(Matrix4d mat) {
	Matrix3d m3 = new Matrix3d();
	mat.get(m3);
	return matrixToEuler(m3);
}
 

/**
 * Extract the translational vector of a transformation matrix.
 *
 * @param transform
 *            Matrix4d
 * @return Vector3d translation vector
 */
public static Vector3d getTranslationVector(Matrix4d transform) {
	Vector3d transl = new Vector3d();
	transform.get(transl);
	return transl;
}