Computes the angle of rotation between two pointing vectors on the ground plane and adds it to a list.

@param pointingPlane Pointing vector of observation in plane reference frame
540338
private void computeAngleOfRotation(PointTrack t, Vector3D_F64 pointingPlane) {
		VoTrack p = t.getCookie();

		// Compute ground pointing vector
		groundCurr.x = pointingPlane.z;
		groundCurr.y = -pointingPlane.x;
		double norm = groundCurr.norm();
		groundCurr.x /= norm;
		groundCurr.y /= norm;

		// dot product.  vectors are normalized to 1 already
		double dot = groundCurr.x * p.ground.x + groundCurr.y * p.ground.y;
		// floating point round off error some times knocks it above 1.0
		if (dot > 1.0)
			dot = 1.0;
		double angle = Math.acos(dot);

		// cross product to figure out direction
		if (groundCurr.x * p.ground.y - groundCurr.y * p.ground.x > 0)
			angle = -angle;

		farAngles.add(angle);
	}
286701
public void rotate(double cos, double sin, Point2D rotationCenter) {
		Transformation2D temp = new Transformation2D();
		temp.setRotate(cos, sin, rotationCenter);
		multiply(temp);
	}
1283986
private void calcPrincipalRotationVector() {
		Rotation rotation = rotationGroup.getRotation(0); // the rotation around the principal axis is the first rotation
		AxisAngle4d axisAngle = rotation.getAxisAngle();
		principalRotationVector = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
	}
202827
private double computeAngle(RoadPolyline segment, boolean fromStartPoint) {
		double angle = Double.NaN;
		Point2d p1 = null;
		Point2d p2 = null;
		if (segment.getPointCount() > 1) {
			if (fromStartPoint) {
				final RoadConnection startPoint = segment.getBeginPoint(StandardRoadConnection.class);
				if (startPoint == this) {
					p1 = startPoint.getPoint();
					p2 = segment.getPointAt(1);
					// Because the Y axis is directly to the bottom of the screen
					// it is necessary to invert it to reply an counterclockwise angle
					angle = MathUtil.clampCyclic(
							Vector2D.signedAngle(
									1, 0, p2.getX() - p1.getX(), p1.getY() - p2.getY()),
							0, MathConstants.TWO_PI);
				} else {
					throw new IllegalArgumentException(
							Locale.getString("ERROR_ALREADY_CONNECTED_START", //$NON-NLS-1$
									segment,
									p1,
									p2,
									startPoint));
				}
			} else {
				final RoadConnection endPoint = segment.getEndPoint(StandardRoadConnection.class);
				if (endPoint == this) {
					p1 = endPoint.getPoint();
					p2 = segment.getAntepenulvianPoint();
					// Because the Y axis is directly to the bottom of the screen
					// it is necessary to invert it to reply an counterclockwise angle
					angle = MathUtil.clampCyclic(
							Vector2D.signedAngle(
									1, 0, p2.getX() - p1.getX(), p1.getY() - p2.getY()),
							0, MathConstants.TWO_PI);
				} else {
					throw new IllegalArgumentException(
							Locale.getString("ERROR_ALREADY_CONNECTED_END", //$NON-NLS-1$
									segment,
									p1,
									p2,
									endPoint));
				}
			}

			if (Double.isNaN(angle) || Double.isInfinite(angle)) {
				throw new IllegalArgumentException(
						Locale.getString("ERROR_SAME_POINTS", //$NON-NLS-1$
								segment, p1, p2));
			}
		} else {
			throw new IllegalArgumentException(
					Locale.getString("ERROR_NOT_ENOUGH_POINTS", //$NON-NLS-1$
							segment));
		}

		return angle;
	}
1053222
void filterPointsOnPlaneAtInfinity(List<DMatrixRMaj> homographies_view1_to_view0,
											   List<List<PointIndex2D_F64>> observations,
											   int totalFeatures ) {
		filtered.clear();
		for (int viewIdx = 0; viewIdx < homographies_view1_to_view0.size(); viewIdx++) {
			List<PointIndex2D_F64> filter = new ArrayList<>();
			filtered.add(filter);
			DMatrixRMaj H = homographies_view1_to_view0.get(viewIdx);

			List<PointIndex2D_F64> obs = observations.get(viewIdx);
			for (int i = 0; i < obs.size(); i++) {
				PointIndex2D_F64 p = obs.get(i);

				if( p.index < 0 || p.index >= totalFeatures)
					throw new IllegalArgumentException("Feature index outside of bounds. Must be from 0 to "+(totalFeatures-1));

				GeometryMath_F64.mult(H,p,tmp);
				// Homogenous coordinates are scale invariant. A scale
				// needs to be picked for consistency. I picked the largest x or y value
				double m = Math.max(Math.abs(tmp.x),Math.abs(tmp.y));
				 if( m == 0 ) m = 1;
				tmp.z /= m;

				// See if it's zero or almost zero, meaning it's on the plane at infinity
				if( Math.abs(tmp.z) > infinityThreshold ) {
					filter.add(p);
				}
			}
		}
	}
242662
public void set(Vector2f start, Vector2f end) {
		super.pointsDirty = true;
		if (this.start == null) {
			this.start = new Vector2f();
		}
		this.start.set(start);

		if (this.end == null) {
			this.end = new Vector2f();
		}
		this.end.set(end);

		vec = new Vector2f(end);
		vec.sub(start);

		lenSquared = vec.lengthSquared();
	}
374239
private Vector3d getReferenceAxisCylicWithSubunitAlignment() {
		if (rotationGroup.getPointGroup().equals("C2")) {
			return referenceVector;
		}

		// find subunit that extends the most in the xy-plane
		List<List<Integer>> orbits = getOrbitsByXYWidth();
		// get the last orbit which is the widest
		List<Integer> widestOrbit = orbits.get(orbits.size()-1);
		List<Point3d> centers = subunits.getCenters();
		int subunit = widestOrbit.get(0);

		// calculate reference vector
		Vector3d refAxis = new Vector3d();
		refAxis.sub(centers.get(subunit), subunits.getCentroid());
		refAxis.normalize();
		return refAxis;
	}
1248050
private void calcReferenceVector() {
		referenceVector = null;
		if (rotationGroup.getPointGroup().startsWith("C")) {
			referenceVector = getReferenceAxisCylic();
		} else if (rotationGroup.getPointGroup().startsWith("D")) {
			referenceVector = getReferenceAxisDihedral();
		} else if (rotationGroup.getPointGroup().equals("T")) {
			referenceVector = getReferenceAxisTetrahedral();
		} else if (rotationGroup.getPointGroup().equals("O")) {
			referenceVector = getReferenceAxisOctahedral();
		} else if (rotationGroup.getPointGroup().equals("I")) {
			referenceVector = getReferenceAxisIcosahedral();
		} else if (rotationGroup.getPointGroup().equals("Helical")) {
			// TODO what should the reference vector be??
			referenceVector = getReferenceAxisCylic();
		}

		if (referenceVector == null) {
			logger.warn("no reference vector found. Using y-axis.");
			referenceVector = new Vector3d(Y_AXIS);
		}
		// make sure reference vector is perpendicular principal roation vector
		referenceVector = orthogonalize(principalRotationVector, referenceVector);
	}
475612
@Override
	public void setCenter(Point3D center) {
		this.cxProperty.set(center.getX());
		this.cyProperty.set(center.getY());
		this.czProperty.set(center.getZ());
	}
900638
private void sortTracksForEstimation() {
		// reset data structures
		planeSamples.reset();
		farAngles.reset();
		tracksOnPlane.clear();
		tracksFar.clear();

		// list of active tracks
		List<PointTrack> active = tracker.getActiveTracks(null);

		for (PointTrack t : active) {
			VoTrack p = t.getCookie();

			// compute normalized image coordinate
			pixelToNorm.compute(t.x, t.y, n);

			// rotate pointing vector into plane reference frame
			pointing.set(n.x, n.y, 1);
			GeometryMath_F64.mult(cameraToPlane.getR(), pointing, pointing);
			pointing.normalize();

			if (p.onPlane) {
				// see if it still intersects the plane
				if (pointing.y > 0) {
					// create data structure for robust motion estimation
					PlanePtPixel ppp = planeSamples.grow();
					ppp.normalizedCurr.set(n);
					ppp.planeKey.set(p.ground);

					tracksOnPlane.add(t);
				}
			} else {
				// if the point is not on the plane visually and (optionally) if it passes a strict y-axis rotation
				// test, consider using the point for estimating rotation.
				boolean allGood = pointing.y < 0;
				if (strictFar) {
					allGood = isRotationFromAxisY(t, pointing);
				}
				// is it still above the ground plane and only has motion consistent with rotation on ground plane axis
				if (allGood) {
					computeAngleOfRotation(t, pointing);
					tracksFar.add(t);
				}
			}
		}
	}
892893
@Override
	public void set(Point3D p1, Point3D p2, Point3D p3) {
		set(p1.getX(), p1.getY(), p1.getZ(), p2.getX(), p2.getY(), p2.getZ(), p3.getX(), p3.getY(), p3.getZ());
	}
1006128
public static void computePosition (
        Point center, double xradius, double yradius, double angle, Point pos)
    {
        pos.x = (int)Math.round(center.x + xradius * Math.cos(angle));
        pos.y = (int)Math.round(center.y + yradius * Math.sin(angle));
    }
392513
private boolean checkHaworthAlignment(Point2d[] points) {
        
        for (int i = 0; i < points.length; i++) {
            Point2d curr = points[i];
            Point2d next = points[(i+1) % points.length];
            
            double deltaY = curr.y - next.y;
            
            if (Math.abs(deltaY) < CARDINALITY_THRESHOLD)
                return true;
        }
        
        return false;
    }
1212098
@Override
	public void setCenter(Point3D center1) {
		setCenter(center1.getX(), center1.getY(), center1.getZ());
	}
661944
public Point rotateAndScalePoint(int x, int y, Point reuse) {
		return applyMatrixToPoint(x, y, reuse, mRotateAndScaleMatrix, mOrientation != 0);
	}
333571
private void calcPrincipalRotationVector() {
//		AxisAngle4d axisAngle = helixLayers.getByLowestAngle().getAxisAngle();
		AxisAngle4d axisAngle = helixLayers.getByLargestContacts().getAxisAngle();
		principalRotationVector = new Vector3d(axisAngle.x, axisAngle.y, axisAngle.z);
	}
68170
private void refineReferenceVector() {
		referenceVector = new Vector3d(Y_AXIS);
		if (rotationGroup.getPointGroup().startsWith("C")) {
			referenceVector = getReferenceAxisCylicWithSubunitAlignment();
		} else if (rotationGroup.getPointGroup().startsWith("D")) {
			referenceVector = getReferenceAxisDihedralWithSubunitAlignment();
		}

		referenceVector = orthogonalize(principalRotationVector, referenceVector);
	}