Java Code Examples for javax.vecmath.Vector3d#lengthSquared()

protected void conditionalChange() {
	if(lock.isLocked()) return;
	lock.lock();
		
	Vector3d oldValue = e.get(); 
	Vector3d newValue = new Vector3d(
		getField(0,oldValue.x),
		getField(1,oldValue.y),
		getField(2,oldValue.z)
	);

	Vector3d diff = new Vector3d();
	diff.sub(newValue,oldValue);
	
	if(diff.lengthSquared()>1e-6) {
		ro.undoableEditHappened(new UndoableEditEvent(this,new ActionChangeVector3d(e, newValue) ) );
	}
	
	lock.unlock();
}
 

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

public double getVectorDistanceKMSq(final EarthVector loc) {
  final Vector3d delta = getVector(loc);

  return delta.lengthSquared();
}
 

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

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