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

protected void testStart() {
	for(int i=0;i<links.size();++i) {
		DHLink bone=links.get(i);
		// save the initial theta for each link
		thetaAtTestStart[i] = links.get(i).getTheta();
		// Use the poseWorld for each DHLink to adjust the model origins.
		bone.refreshPoseMatrix();
		bone.setModel(models[i].getModel());
		bone.setMaterial(models[i].getMaterial());
		Matrix4d iWP = bone.getPoseWorld();
		iWP.invert();
		if(bone.getModel()!=null) {
			bone.getModel().adjustMatrix(iWP);
		}
		// only allow theta adjustments of DH parameters
		bone.flags = LinkAdjust.THETA;
	}
	endEffectorTarget.setPoseWorld(endEffector.getPoseWorld());
	
	getPoseFK(poseFKold);
	poseFKnew.set(poseFKold);
}
 

protected void rotationInternal(Matrix4d rotation) {
	// multiply robot origin by target matrix to get target matrix in world space.
	
	// invert frame of reference to transform world target matrix into frame of reference space.
	Matrix4d ifor = new Matrix4d(FOR);
	ifor.invert();

	Matrix4d subjectRotation = new Matrix4d(startMatrix);		
	Matrix4d subjectAfterRotation = new Matrix4d(FOR);
	subjectAfterRotation.mul(rotation);
	subjectAfterRotation.mul(ifor);
	subjectAfterRotation.mul(subjectRotation);

	resultMatrix.set(subjectAfterRotation);
	resultMatrix.setTranslation(MatrixHelper.getPosition(startMatrix));
}
 

/**
 * transform a world-space point to the ball's current frame of reference
 * @param pointInWorldSpace the world space point
 * @return the transformed Vector3d
 */
public Vector3d getPickPointInFOR(Vector3d pointInWorldSpace,Matrix4d frameOfReference) {
	Matrix4d iMe = new Matrix4d(frameOfReference);
	iMe.m30=iMe.m31=iMe.m32=0;
	iMe.invert();
	Vector3d pickPointInBallSpace = new Vector3d(pointInWorldSpace);
	pickPointInBallSpace.sub(this.getPosition());
	iMe.transform(pickPointInBallSpace);
	return pickPointInBallSpace;
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the travel for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	
	Matrix4d m4 = new Matrix4d(targetPoseAdj);

	keyframe.fkValues[0] = m4.m23;
	keyframe.fkValues[1] = m4.m03;
	keyframe.fkValues[2] = m4.m13;
 
	
	if(true) {
		Log.message("solution={"+StringHelper.formatDouble(keyframe.fkValues[0])+","+
				StringHelper.formatDouble(keyframe.fkValues[1])+","+
				StringHelper.formatDouble(keyframe.fkValues[2])+"}");
	}
	return SolutionType.ONE_SOLUTION;
}
 

@Override
public void setPoseWorld(Matrix4d newPose) {
	Matrix4d newRelativePose;
	if(parent instanceof PoseEntity) {
		PoseEntity pe = (PoseEntity)parent;
		newRelativePose=pe.getPoseWorld();
		newRelativePose.invert();
		newRelativePose.mul(newPose);
	} else {
		newRelativePose=newPose;
	}
	
	setPose(newRelativePose);
}
 

public void renderShared(GL2 gl2) {
	// store the projection matrix for later
       double [] m = new double[16];
       gl2.glGetDoublev(GL2.GL_PROJECTION_MATRIX, m, 0);
       projectionMatrix.set(m);

   	gl2.glMatrixMode(GL2.GL_MODELVIEW);
       gl2.glLoadIdentity();
   	
	PoseEntity camera = getAttachedTo();
	Matrix4d mFinal = camera.getPoseWorld();
	mFinal.invert();
	MatrixHelper.applyMatrix(gl2, mFinal);
}
 

/**
 * Set the pose and poseWorld of this item
 * @param m
 */
public void setPoseWorld(Matrix4d m) {
	if(parent instanceof PoseEntity) {
		PoseEntity pep = (PoseEntity)parent;
		Matrix4d newPose = new Matrix4d(pep.poseWorld);
		newPose.invert();
		newPose.mul(m);
		setPose(newPose);
	} else {
		setPose(new Matrix4d(m));
	}
}
 

/**
 * This method is the main function call to extract all step parameters, pairing parameters, and sequence
 * information from the Structure object provided to the constructor.
 * @return This same object with the populated data, convenient for output
 *  (e.g. <i>log.info(new BasePairParameters(structure).analyze());</i>)
 */
public BasePairParameters analyze() {
	if (structure == null) {
		pairingParameters = null;
		stepParameters = null;
		return this;
	}
	List<Chain> nucleics = this.getNucleicChains(nonredundant);
	List<Pair<Group>> pairs = this.findPairs(nucleics);
	this.pairingParameters = new double[pairs.size()][6];
	this.stepParameters = new double[pairs.size()][6];
	Matrix4d lastStep;
	Matrix4d currentStep = null;
	for (int i = 0; i < pairs.size(); i++) {
		lastStep = currentStep;
		currentStep = this.basePairReferenceFrame(pairs.get(i));
		referenceFrames.add((Matrix4d)currentStep.clone());
		for (int j = 0; j < 6; j++) pairingParameters[i][j] = pairParameters[j];
		if (i != 0) {
			lastStep.invert();
			lastStep.mul(currentStep);
			double[] sparms = calculateTp(lastStep);
			for (int j = 0; j < 6; j++) stepParameters[i][j] = sparms[j];
		}
	}
	return this;
}
 

/**
 * Recursive helper
 * @param symmAxes output list
 * @param prior transformation aligning the first repeat of this axis with the first overall
 * @param level current level
 */
private void getSymmetryAxes(List<Axis> symmAxes, Matrix4d prior, int level, int firstRepeat) {
	if(level >= getNumLevels() ) {
		return;
	}

	Axis elem = axes.get(level);
	Matrix4d elemOp = elem.getOperator();

	// Current axis:
	// elementary maps B -> A
	// prior maps I -> A and J -> B
	// want J -> I = J -> B -> A <- I= inv(prior) * elementary * prior
	Matrix4d currAxisOp = new Matrix4d(prior);
	currAxisOp.invert();
	currAxisOp.mul(elemOp);
	currAxisOp.mul(prior);
	Axis currAxis = new Axis(currAxisOp,elem.getOrder(),elem.getSymmType(),level,firstRepeat);
	symmAxes.add(currAxis);

	//Remember that all degrees are at least 2
	getSymmetryAxes(symmAxes,prior,level+1,firstRepeat);
	//New prior is elementary^d*prior
	Matrix4d newPrior = new Matrix4d(elemOp);
	newPrior.mul(prior);
	int childSize = getNumRepeats(level+1);
	getSymmetryAxes(symmAxes,newPrior,level+1,firstRepeat+childSize);
	for(int d=2;d<elem.getOrder();d++) {
		newPrior.mul(elemOp,newPrior);
		getSymmetryAxes(symmAxes,newPrior,level+1,firstRepeat+childSize*d);
	}
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the angles for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@SuppressWarnings("unused")
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	DHLink link4 = robot.links.get(robot.links.size()-1);

	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	
	Matrix4d m4 = new Matrix4d(targetPoseAdj);
	
	Point3d p4 = new Point3d(m4.m03,m4.m13,m4.m23);
	
	// the the base rotation
	keyframe.fkValues[0]=Math.toDegrees(Math.atan2(p4.x,-p4.y));

	// the height
	keyframe.fkValues[1]=p4.z;
	
	// the distance out from the center
	keyframe.fkValues[2] = Math.sqrt(p4.x*p4.x + p4.y*p4.y);
	
	// the rotation at the end effector
	Vector4d relativeX = new Vector4d(p4.x,p4.y,0,0);
	relativeX.scale(1/keyframe.fkValues[2]);  // normalize it
	
	Vector4d relativeY = new Vector4d(-relativeX.y,relativeX.x,0,0);

	Vector4d m4x = new Vector4d();
	m4.getColumn(1, m4x);
	
	double rX = m4x.dot(relativeX);
	double rY = m4x.dot(relativeY);
	
	keyframe.fkValues[3] = Math.toDegrees(-Math.atan2(rY,rX));
	
	if(false) {
		Log.message("solution={"+StringHelper.formatDouble(keyframe.fkValues[0])+","+
							keyframe.fkValues[1]+","+
							keyframe.fkValues[2]+","+
							StringHelper.formatDouble(keyframe.fkValues[3])+"}");
	}
	return SolutionType.ONE_SOLUTION;
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the angles for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@SuppressWarnings("unused")
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	DHLink link0 = robot.links.get(0);
	DHLink link1 = robot.links.get(1);
	DHLink link2 = robot.links.get(2);
	DHLink link3 = robot.links.get(3);
	DHLink link4 = robot.links.get(4);

	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	
	Vector3d p4 = new Vector3d(
			targetPoseAdj.m03,
			targetPoseAdj.m13,
			targetPoseAdj.m23);

	// Work forward to get p1 position
	Point3d p1 = new Point3d(0,0,link0.getD());
	
	// (1) theta0 = atan(y07/x07);
	keyframe.fkValues[0] = Math.toDegrees(Math.atan2(p4.x,-p4.y));  // TODO explain why this isn't Math.atan2(p7.y,p7.x)
	if(false) Log.message("t0="+keyframe.fkValues[0]+"\t");
	
	// (2) C=z14
	double c = p4.z - p1.z;
	if(false) Log.message("c="+c+"\t");
	
	// (3) 
	double x15 = p4.x-p1.x;
	double y15 = p4.y-p1.y;
	double d = Math.sqrt(x15*x15 + y15*y15);
	if(false) Log.message("d="+d+"\t");
	
	// (4)
	double e = Math.sqrt(c*c + d*d);
	if(false) Log.message("e="+e+"\t");

	// (5) phi = acos( (b^2 - a^2 - e^2) / (-2*a*e) ) 
	double a = link2.getD();
	double b2 = link4.getD();
	double b1 = link3.getD();
	double b = Math.sqrt(b2*b2+b1*b1);
	if(false) Log.message("b="+b+"\t");
	
	double phi = Math.acos( (b*b-a*a-e*e) / (-2*a*e) );
	if(false) Log.message("phi="+Math.toDegrees(phi)+"\t");
	
	// (6) rho = atan2(d,c)
	double rho = Math.atan2(d,c);
	if(false) Log.message("rho="+Math.toDegrees(rho)+"\t");
	
	// (7) alpha1 = phi-rho
	keyframe.fkValues[1] = Math.toDegrees(rho - phi);
	if(false) Log.message("a1="+keyframe.fkValues[1]+"\t");
	
	// (8) omega = acos( (a^2-b^2-e^2) / (-2be) )
	double omega = Math.acos( (a*a-b*b-e*e) / (-2*b*e) );
	if(false) Log.message("omega="+Math.toDegrees(omega)+"\t");

	// (9) phi3 = phi + omega
	double phi3 = phi+omega;
	if(false) Log.message("phi3="+Math.toDegrees(phi3)+"\t");
			
	// angle of triangle j3-j2-j5 is ph4.
	// b2^2 = b^+b1^2-2*b*b1*cos(phi4)
	double phi4 = Math.acos( (b2*b2-b1*b1-b*b) / (-2*b1*b) );
	if(false) Log.message("phi4="+Math.toDegrees(phi4)+"\t");
	
	// (10) alpha2 - phi3-phi4
	keyframe.fkValues[2] = Math.toDegrees(phi3 - phi4);
	if(false) Log.message("alpha2="+keyframe.fkValues[2]+"\t");
	
	if(true) Log.message("solution={"+keyframe.fkValues[0]+","+keyframe.fkValues[1]+","+keyframe.fkValues[2]+"}");
	
	return SolutionType.ONE_SOLUTION;
}
 

/**
 * Starting from a known local origin and a known local hand position, calculate the angles for the given pose.
 * @param robot The DHRobot description. 
 * @param targetMatrix the pose that robot is attempting to reach in this solution.
 * @param keyframe store the computed solution in keyframe.
 */
@SuppressWarnings("unused")
@Override
public SolutionType solve(DHRobotEntity robot,Matrix4d targetMatrix,DHKeyframe keyframe) {
	DHLink link4 = robot.links.get(robot.links.size()-1);

	Matrix4d targetPoseAdj = new Matrix4d(targetMatrix);
	
	if(robot.dhTool!=null) {
		// there is a transform between the wrist and the tool tip.
		// use the inverse to calculate the wrist Z axis and wrist position.
		robot.dhTool.refreshPoseMatrix();
		Matrix4d inverseToolPose = new Matrix4d(robot.dhTool.getPose());
		inverseToolPose.invert();
		targetPoseAdj.mul(inverseToolPose);
	}
	Matrix4d m4 = new Matrix4d(targetPoseAdj);
	
	Point3d p4 = new Point3d(m4.m03,m4.m13,m4.m23);

	double a1 = robot.links.get(0).getR();
	double a2 = robot.links.get(1).getR();
	
	double b = a1;
	double a = a2;
	double c = Math.sqrt(p4.x*p4.x+p4.y*p4.y);
	if(c>a+b) c=a+b;

	// law of cosines to get the angle at the elbow
	// phi = acos( (b^2 - a^2 - c^2) / (-2*a*c) )
	double phi = Math.acos((b*b+a*a-c*c)/(2*a*b));

	keyframe.fkValues[1]=180-Math.toDegrees(phi);  // TODO explain this 180- here.
	
	// The the base rotation
	double theta2 = Math.PI - phi;
	double cc = a2 * Math.sin(theta2);
	double bb = a2 * Math.cos(theta2) + a1;
	double aa = c;
	double phi2 = Math.atan2(cc,bb);
	double tau = Math.atan2(p4.y,p4.x); 
	keyframe.fkValues[0]=Math.toDegrees(tau-phi2); 
	
	Point3d p3 = new Point3d(p4);
	p3.z = robot.links.get(0).getD();
	Point3d p2 = new Point3d(Math.cos(phi),Math.sin(phi),p3.z);
	
	// the height
	keyframe.fkValues[2]=-(p3.z-p4.z);
	
	// the rotation at the end effector
	Vector4d relativeX = new Vector4d(p3.x-p2.x,p3.y-p2.y,0,0);
	relativeX.normalize();  // normalize it
	
	Vector4d relativeY = new Vector4d(-relativeX.y,relativeX.x,0,0);

	Vector4d m4x = new Vector4d();
	m4.getColumn(1, m4x);
	
	double rX = m4x.dot(relativeX);
	double rY = m4x.dot(relativeY);
	
	keyframe.fkValues[3] = Math.toDegrees(-Math.atan2(rY,rX));
	
	if(false) {
		Log.message("solution={"+StringHelper.formatDouble(keyframe.fkValues[0])+","+
							keyframe.fkValues[1]+","+
							keyframe.fkValues[2]+","+
							StringHelper.formatDouble(keyframe.fkValues[3])+"}");
	}
	return SolutionType.ONE_SOLUTION;
}
 

/**
 * Checks whether given interface is NCS-redundant, i.e., an identical interface between NCS copies of
 * these molecules has already been seen, and returns this (reference) interface.
 *
 * @param interf
 *          StructureInterface
 * @return  already seen interface that is NCS-equivalent to interf,
 *          null if such interface is not found.
 */
private StructureInterface findNcsRef(StructureInterface interf) {
	if (!this.hasNcsOps()) {
		return null;
	}
	String chainIName = interf.getMoleculeIds().getFirst();
	String iOrigName = chainOrigNames.get(chainIName);

	String chainJName = interf.getMoleculeIds().getSecond();
	String jOrigName = chainOrigNames.get(chainJName);

	Matrix4d mJCryst;
	if(this.searchBeyondAU) {
		mJCryst = interf.getTransforms().getSecond().getMatTransform();
		mJCryst = crystallographicInfo.getCrystalCell().transfToOrthonormal(mJCryst);
	} else {
		mJCryst = IDENTITY;
	}

	// Let X1,...Xn be the original coords, before NCS transforms (M1...Mk)
	// current chain i: M_i * X_i
	// current chain j: Cn * M_j * X_j

	// transformation to bring chain j near X_i: M_i^(-1) * Cn * M_j
	// transformation to bring chain i near X_j: (Cn * M_j)^(-1) * M_i = (M_i^(-1) * Cn * M_j)^(-1)

	Matrix4d mChainIInv = new Matrix4d(chainNcsOps.get(chainIName));
	mChainIInv.invert();

	Matrix4d mJNcs = new Matrix4d(chainNcsOps.get(chainJName));

	Matrix4d j2iNcsOrigin = new Matrix4d(mChainIInv);
	j2iNcsOrigin.mul(mJCryst);
	//overall transformation to bring current chainj from its NCS origin to i's
	j2iNcsOrigin.mul(mJNcs);

	//overall transformation to bring current chaini from its NCS origin to j's
	Matrix4d i2jNcsOrigin = new Matrix4d(j2iNcsOrigin);
	i2jNcsOrigin.invert();

	String matchChainIdsIJ = iOrigName + jOrigName;
	String matchChainIdsJI = jOrigName + iOrigName;

	// same original chain names
	Optional<Matrix4d> matchDirect =
			visitedNcsChainPairs.computeIfAbsent(matchChainIdsIJ, k-> new HashMap<>()).entrySet().stream().
				map(r->r.getKey()).
				filter(r->r.epsilonEquals(j2iNcsOrigin,0.01)).
				findFirst();

	Matrix4d matchMatrix = matchDirect.orElse(null);
	String matchChainIds = matchChainIdsIJ;

	if(matchMatrix == null) {
		// reversed original chain names with inverted transform
		Optional<Matrix4d> matchInverse =
				visitedNcsChainPairs.computeIfAbsent(matchChainIdsJI, k-> new HashMap<>()).entrySet().stream().
				map(r->r.getKey()).
				filter(r->r.epsilonEquals(i2jNcsOrigin,0.01)).
				findFirst();
		matchMatrix = matchInverse.orElse(null);
		matchChainIds = matchChainIdsJI;
	}

	StructureInterface matchInterface = null;

	if (matchMatrix == null) {
		visitedNcsChainPairs.get(matchChainIdsIJ).put(j2iNcsOrigin,interf);
	} else {
		matchInterface = visitedNcsChainPairs.get(matchChainIds).get(matchMatrix);
	}

	return matchInterface;
}
 

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