javax.vecmath.AxisAngle4d Java Examples

public void setC1(int n) {
	Rotation r = new Rotation();
	List<Integer> permutation = new ArrayList<Integer>(n);
	for (int i = 0; i < n; i++) {
		permutation.add(i);
	}
	r.setPermutation(permutation);
	Matrix4d m = new Matrix4d();
	m.setIdentity();
	r.setTransformation(m);
	r.setAxisAngle(new AxisAngle4d());
	r.setFold(1);
	r.setScores(new QuatSymmetryScores());
	rotations.add(r);
	pointGroup = "C1";
}
 

private Rotation createSymmetryOperation(List<Integer> permutation, Matrix4d transformation, AxisAngle4d axisAngle, int fold, QuatSymmetryScores scores) {
	Rotation s = new Rotation();
	s.setPermutation(new ArrayList<Integer>(permutation));
	s.setTransformation(new Matrix4d(transformation));
	s.setAxisAngle(new AxisAngle4d(axisAngle));
	s.setFold(fold);
	s.setScores(scores);
	return s;
}
 

/**
 * Rotates this coordinate about the input vector through the input angle (radians - because we
 * usually use this internally)
 *
 * @param rotAxis The axis of rotation
 * @param angle The angle of rotation (in radians)
 */
public Vector3d rotate(final Vector3d rotAxis, final double angle) {
  final Vector3d thisVec = new Vector3d(ecfVector);
  final Vector3d axis = new Vector3d(rotAxis);
  axis.normalize();

  final Matrix3d trans = new Matrix3d();
  trans.set(new AxisAngle4d(axis, angle));

  trans.transform(thisVec);

  return thisVec;
}
 

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

private void addEOperation() {
	List<Integer> permutation = Arrays.asList(0,1);
	Matrix4d transformation = new Matrix4d();
	transformation.setIdentity();
	combineWithTranslation(transformation);
	AxisAngle4d axisAngle = new AxisAngle4d();
	QuatSymmetryScores scores = new QuatSymmetryScores();
	int fold = 1; // ??
	Rotation rotation = createSymmetryOperation(permutation, transformation, axisAngle, fold, scores);
	rotations.addRotation(rotation);
}
 

/**
 * Add E operation to the highest order rotation axis. By definition
 * E belongs to the highest order axis.
 */
private void setEAxis() {
	Rotation e = rotations.get(0);
	Rotation h = rotations.get(principalAxisIndex);
	e.setAxisAngle(new AxisAngle4d(h.getAxisAngle()));
	e.getAxisAngle().angle = 0.0;
	e.setFold(h.getFold());
}
 

private Rotation createSymmetryOperation(List<Integer> permutation, Matrix4d transformation, AxisAngle4d axisAngle, int fold, QuatSymmetryScores scores) {
	Rotation s = new Rotation();
	s.setPermutation(new ArrayList<Integer>(permutation));
	s.setTransformation(new Matrix4d(transformation));
	s.setAxisAngle(new AxisAngle4d(axisAngle));
	s.setFold(fold);
	s.setScores(scores);
	return s;
}
 

/**
 * Given a rotation matrix calculates the rotation axis and angle for it.
 * The angle is calculated from the trace, the axis from the eigenvalue
 * decomposition.
 * If given matrix is improper rotation or identity matrix then
 * axis (0,0,0) and angle 0 are returned.
 * @param m
 * @return
 * @throws IllegalArgumentException if given matrix is not a rotation matrix (determinant not 1 or -1)
 */
public static AxisAngle4d getRotAxisAndAngle(Matrix3d m) {
	double determinant = m.determinant();

	if (!(Math.abs(determinant)-1.0<DELTA)) throw new IllegalArgumentException("Given matrix is not a rotation matrix");

	AxisAngle4d axisAndAngle = new AxisAngle4d(new Vector3d(0,0,0),0);

	double[] d = {m.m00,m.m10,m.m20,
			m.m01,m.m11,m.m21,
			m.m02,m.m12,m.m22};

	Matrix r = new Matrix(d,3);

	if (!deltaComp(r.det(), 1.0, DELTA)) {
		// improper rotation: we return axis 0,0,0 and angle 0
		return axisAndAngle;
	}

	EigenvalueDecomposition evd = new EigenvalueDecomposition(r);

	Matrix eval = evd.getD();
	if (deltaComp(eval.get(0, 0),1.0,DELTA) && deltaComp(eval.get(1, 1),1.0,DELTA) && deltaComp(eval.get(2, 2),1.0,DELTA)) {
		// the rotation is an identity: we return axis 0,0,0 and angle 0
		return axisAndAngle;
	}
	int indexOfEv1;
	for (indexOfEv1=0;indexOfEv1<3;indexOfEv1++) {
		if (deltaComp(eval.get(indexOfEv1, indexOfEv1),1,DELTA)) break;
	}
	Matrix evec = evd.getV();
	axisAndAngle.set(new Vector3d(evec.get(0,indexOfEv1), evec.get(1, indexOfEv1), evec.get(2, indexOfEv1)),
			Math.acos((eval.trace()-1.0)/2.0));

	return axisAndAngle;
}
 

private void a(Vector3f vector, clz var1) {
	if (var1 != null) {
		Matrix4d var2 = this.a();
		Vector3f var3 = new Vector3f(0.0F, 0.0F, 0.0F);
		switch (var1.b) {
			case a:
				var2.mul(a(new AxisAngle4d(1.0D, 0.0D, 0.0D, var1.c * 0.017453292519943295D)));
				var3.set(0.0F, 1.0F, 1.0F);
				break;
			case b:
				var2.mul(a(new AxisAngle4d(0.0D, 1.0D, 0.0D, var1.c * 0.017453292519943295D)));
				var3.set(1.0F, 0.0F, 1.0F);
				break;
			case c:
				var2.mul(a(new AxisAngle4d(0.0D, 0.0D, 1.0D, var1.c * 0.017453292519943295D)));
				var3.set(1.0F, 1.0F, 0.0F);
		}

		if (var1.d) {
			if (Math.abs(var1.c) == 22.5F) {
				var3.scale(fieldA);
			} else {
				var3.scale(fieldB);
			}

			var3.add(new Vector3f(1.0F, 1.0F, 1.0F));
		} else {
			var3.set(1.0F, 1.0F, 1.0F);
		}

		this.a(vector, new Vector3f(var1.a), var2, var3);
	}
}
 

private static Rotation createSymmetryOperation(List<Integer> permutation, Matrix4d transformation, AxisAngle4d axisAngle, int fold, QuatSymmetryScores scores) {
	Rotation s = new Rotation();
	s.setPermutation(new ArrayList<Integer>(permutation));
	s.setTransformation(new Matrix4d(transformation));
	s.setAxisAngle(new AxisAngle4d(axisAngle));
	s.setFold(fold);
	s.setScores(scores);

	return s;
}
 

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

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

private void calcRotAxesAndAngles() {

		axisAngles = new AxisAngle4d[multiplicity];

		// identity operator (transformId==0)
		axisAngles[0] = new AxisAngle4d(new Vector3d(0,0,0), 0.0);

		for (int i=1;i<this.transformations.size();i++){
			Matrix3d r = new Matrix3d(transformations.get(i).m00,transformations.get(i).m01,transformations.get(i).m02,
					transformations.get(i).m10,transformations.get(i).m11,transformations.get(i).m12,
					transformations.get(i).m20,transformations.get(i).m21,transformations.get(i).m22);

			axisAngles[i] = getRotAxisAndAngle(r);

		}
	}
 

public AxisAngle4d getRotAxisAngle(int transformId) {
	if (this.axisAngles == null) calcRotAxesAndAngles();
	return this.axisAngles[transformId];
}
 

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

}
 

@Override
public void update(long t) {
    if (lastTime >= 0) {
        double dt = (t - lastTime) / 1000.0;
        if (dt < 0.001) {
            dt = 0.001; // Limit min dt to 1ms
        }
        // Position
        Vector3d dPos = new Vector3d(velocity);
        dPos.scale(dt);
        position.add(dPos);
        // Velocity
        acceleration = getForce();
        acceleration.scale(1.0 / mass);
        acceleration.add(getWorld().getEnvironment().getG());
        if (position.z >= getWorld().getEnvironment().getGroundLevel(position) &&
                velocity.z + acceleration.z * dt >= 0.0) {
            // On ground
            acceleration.x = -velocity.x / dt;
            acceleration.y = -velocity.y / dt;
            acceleration.z = -velocity.z / dt;
            position.z = getWorld().getEnvironment().getGroundLevel(position);
            //rotationRate.set(0.0, 0.0, 0.0);
        }
        Vector3d dVel = new Vector3d(acceleration);
        dVel.scale(dt);
        velocity.add(dVel);
        // Rotation
        if (rotationRate.length() > 0.0) {
            Matrix3d r = new Matrix3d();
            Vector3d rotationAxis = new Vector3d(rotationRate);
            rotationAxis.normalize();
            r.set(new AxisAngle4d(rotationAxis, rotationRate.length() * dt));
            rotation.mulNormalize(r);
        }
        // Rotation rate
        Vector3d Iw = new Vector3d(rotationRate);
        momentOfInertia.transform(Iw);
        Vector3d angularAcc = new Vector3d();
        angularAcc.cross(rotationRate, Iw);
        angularAcc.negate();
        angularAcc.add(getTorque());
        momentOfInertiaInv.transform(angularAcc);
        angularAcc.scale(dt);
        rotationRate.add(angularAcc);
    }
    lastTime = t;
}
 

/**
 * Returns the AxisAngle of the helix transformation
 * @param transformation helix transformation
 * @return
 */
public AxisAngle4d getAxisAngle() {
	AxisAngle4d axis = new AxisAngle4d();
	axis.set(this.transformation);
	return axis;
}
 

/**
 * @param axisAngle the axisAngle to set
 */
public void setAxisAngle(AxisAngle4d axisAngle) {
	this.axisAngle = axisAngle;
}
 

/**
 * @return the axisAngle
 */
public AxisAngle4d getAxisAngle() {
	return axisAngle;
}
 

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

public static void getAxisAngle(int index, AxisAngle4d axisAngle) {
	quat.set(orientations[index]);
	axisAngle.set(quat);
}
 

/**
 * Locate a coordinate at a specific distance (km), elevation angle (radians), and heading
 * (radians) from this one.
 */
public EarthVector findPoint(
    final double distanceKM,
    final double azimuth,
    final double elevAngle) {
  // convert distance to radians
  // final double distR = distanceKM / KMPerDegree() / DPR;
  final double lon = getLongitude();
  final double lat = getLatitude();
  // convert local enu to ecf to get east and north vectors
  // east vector
  final Vector3d eastVec = new Vector3d(1, 0, 0);
  final Vector3d northVec = new Vector3d(0, 1, 0);
  final double sinLon = Math.sin(lon);
  final double cosLon = Math.cos(lon);
  final double sinLat = Math.sin(lat);
  final double cosLat = Math.cos(lat);
  final Matrix3d enuToEcf = new Matrix3d();
  enuToEcf.m00 = -sinLon;
  enuToEcf.m01 = -(sinLat * cosLon);
  enuToEcf.m02 = cosLat * cosLon;
  enuToEcf.m10 = cosLon;
  enuToEcf.m11 = -(sinLat * sinLon);
  enuToEcf.m12 = cosLat * sinLon;
  enuToEcf.m20 = 0;
  enuToEcf.m21 = cosLat;
  enuToEcf.m22 = sinLat;
  enuToEcf.transform(eastVec);
  enuToEcf.transform(northVec);
  eastVec.normalize();
  northVec.normalize();
  northVec.scale(distanceKM);
  final Matrix3d elevTrans = new Matrix3d();
  elevTrans.set(new AxisAngle4d(eastVec, elevAngle));

  elevTrans.transform(northVec);
  final Matrix3d azTrans = new Matrix3d();
  final Vector3d unitEcf = new Vector3d(ecfVector);
  unitEcf.normalize();
  azTrans.set(new AxisAngle4d(unitEcf, azimuth));
  azTrans.transform(northVec);
  final Vector3d transformedEcf = new Vector3d();
  transformedEcf.add(ecfVector, northVec);
  final EarthVector transformedEv = new EarthVector(transformedEcf);
  return transformedEv;
}
 

private Matrix4d a(AxisAngle4d paramAxisAngle4d) {
	Matrix4d localMatrix4d = a();
	localMatrix4d.setRotation(paramAxisAngle4d);
	return localMatrix4d;
}
 

/**
 * Returns the rise of a helix given the subunit centers of two adjacent
 * subunits and the helix transformation
 *
 * @param transformation
 *            helix transformation
 * @param p1
 *            center of one subunit
 * @param p2
 *            center of an adjacent subunit
 * @return
 */
private static double getRise(Matrix4d transformation, Point3d p1,
		Point3d p2) {
	AxisAngle4d axis = getAxisAngle(transformation);
	Vector3d h = new Vector3d(axis.x, axis.y, axis.z);
	Vector3d p = new Vector3d();
	p.sub(p1, p2);
	return p.dot(h);
}
 

/**
 * Returns the AxisAngle of the helix transformation
 *
 * @param transformation
 *            helix transformation
 * @return
 */
private static AxisAngle4d getAxisAngle(Matrix4d transformation) {
	AxisAngle4d axis = new AxisAngle4d();
	axis.set(transformation);
	return axis;
}
 

/**
 * Returns the rotation axis and angle in a single javax.vecmath.AxisAngle4d object
 * @return
 */
public AxisAngle4d getAxisAngle4d() {
	return new AxisAngle4d(rotationAxis.getX(),rotationAxis.getY(),rotationAxis.getZ(),theta);
}
 

/**
 * Calculate the rotation angle component of the input unit quaternion.
 *
 * @param q
 *            unit quaternion Quat4d
 * @return the angle in radians of the input quaternion
 */
public static double angle(Quat4d q) {
	AxisAngle4d axis = new AxisAngle4d();
	axis.set(q);
	return axis.angle;
}