lcsim/src/org/lcsim/util/swim
diff -u -r1.4 -r1.5
--- Line.java 26 Aug 2005 21:02:26 -0000 1.4
+++ Line.java 22 Sep 2005 19:16:37 -0000 1.5
@@ -2,11 +2,12 @@
import hep.physics.vec.BasicHep3Vector;
import hep.physics.vec.Hep3Vector;
+import hep.physics.vec.VecOp;
/**
* A straight line
* @author tonyj
- * @version $Id: Line.java,v 1.4 2005/08/26 21:02:26 jstrube Exp $
+ * @version $Id: Line.java,v 1.5 2005/09/22 19:16:37 mcharles Exp $
*/
public class Line implements Trajectory
{
@@ -16,6 +17,12 @@
private double cosLambda;
private Hep3Vector origin;
+ /**
+ * Defines a line in space. The direction of the line is defined
+ * by the angles phi and lambda, such that the Cartesian unit vector is
+ * (cos(lambda)*cos(phi), cos(lambda)*sin(phi), sin(lambda)).
+ * @param origin A point on the line
+ */
public Line(Hep3Vector origin, double phi, double lambda)
{
this.origin = origin;
@@ -25,6 +32,10 @@
cosLambda = Math.cos(lambda);
}
+ /**
+ * Gets a point after traveling distance alpha from the trajectory's
+ * origin point along the trajectory
+ */
public Hep3Vector getPointAtDistance(double alpha)
{
double z = origin.z() + alpha*sinLambda;
@@ -34,19 +45,208 @@
return new BasicHep3Vector(x,y,z);
}
+ /**
+ * Calculates the distance at which the trajectory first reaches radius R.
+ * Returns Double.NaN if the trajectory does not intercept the cylinder.
+ * In principle there could be multiple solutions, so this method should
+ * always return the minimum <b>positive</b> solution.
+ */
public double getDistanceToInfiniteCylinder(double r)
{
- // FIXME: Implement properly
- return Double.NaN;
+ if (cosLambda == 0.0) {
+ // This is a special case where the line is parallel to the z-axis.
+ // There can be no well-defined answer in this case
+ return Double.NaN;
+ } else {
+ // Find the intercepts on the cylinder (Cartesian):
+ double[] distances = this.findInterceptsOnCylinder(r);
+ // Which is the best?
+ double bestDistance = -1.0;
+ for (int i=0; i<distances.length; i++) {
+ if (distances[i] >= 0.0) {
+ // Potentially valid -- is it the best so far?
+ if (bestDistance<0.0 || distances[i]<bestDistance) {
+ // Yes!
+ bestDistance = distances[i];
+ }
+ }
+ }
+ if (bestDistance < 0.0) {
+ // There was no valid (positive) solution
+ return Double.NaN;
+ } else {
+ return bestDistance;
+ }
+ }
}
+ /**
+ * Calculate the distance along the trajectory to reach a given Z plane.
+ * Note distance may be negative.
+ */
public double getDistanceToZPlane(double z)
{
return (z-origin.z())/sinLambda;
}
- // FIXME implementation
+ /**
+ * Calculates the shortest distance of a point to the trajectory
+ * @param point The point in space
+ * @return the distance of closest approach in mm
+ */
public double getDistanceToPoint(Hep3Vector point) {
- return Double.NaN;
+ return findDOCAToPoint(point);
}
+
+ /**
+ * An internal utility routine. This finds all intercepts of the
+ * line on a infinite cylinder of radius r. The cylinder's axis
+ * is the same as the z-axis. The return value is an array of
+ * signed distances from the origin point to the intercept point.
+ * If there are no solutions, the return value is null.
+ */
+ protected double[] findInterceptsOnCylinder(double r)
+ {
+ if (cosLambda == 0.0) {
+ throw new AssertionError("Don't call this private routine with a line that's parallel to the z-axis!");
+ }
+
+ // Treat this first as a problem in the XY plane, then think about Z later.
+ // Track is described as
+ // position(t) = (x0, y0) + t(vx, vy)
+ // i.e.
+ // x = x0 + t(vx)
+ // y = y0 + t(vy) = y0 + ((x-x0)/vx)(vy)
+ // = [y0 - x0.vy/vx] + [x.vy/vx]
+ // = c + m.x
+ // Circle is described as
+ // x*x + y*y = r*r
+ // Solve for x and y:
+ // x*x + (c+mx)*(c+mx) = r*r
+ // x*x + c*c + 2*c*m*x + m*m*x*x = r*r
+ // (m*m + 1).x^2 + (2*c*m).x + (c*c - r*r) = 0
+ // Thus, solving the quadratic,
+ // x = [ -(2*c*m) +- sqrt( (2*c*m)^2 - 4*(m*m+1)*(c*c-r*r) ) ] / [2*(m*m+1)]
+
+ double x0 = origin.x();
+ double y0 = origin.y();
+ double vx = cosLambda*cosPhi;
+ double vy = cosLambda*sinPhi;
+
+ double c = y0 - (x0*vy)/vx;
+ double m = vy/vx;
+
+ if (vx == 0.0) {
+ // This is a special case where the line is perpendicular to the x-axis.
+ // Handle this by flipping the x and y coordinates.
+ x0 = origin.y();
+ y0 = origin.x();
+ vx = cosLambda*sinPhi;
+ vy = cosLambda*cosPhi;
+ c = y0 - (x0*vy)/vx;
+ m = vy/vx;
+ }
+
+ // Solve the quadratic...
+ double termA = (m*m + 1.0);
+ double termB = (2.0*c*m);
+ double termC = (c*c - r*r);
+ double sqrtTerm = termB*termB - 4.0*termA*termC;
+
+ double[] xSolutions = null; // This will hold the co-ordinates of the solutions
+
+ if (sqrtTerm>0.0) {
+ // Two solutions.
+ xSolutions = new double[2];
+ xSolutions[0] = ( -termB + Math.sqrt(sqrtTerm) ) / (2.0 * termA);
+ xSolutions[1] = ( -termB - Math.sqrt(sqrtTerm) ) / (2.0 * termA);
+ } else if (sqrtTerm==0.0) {
+ // One solution
+ xSolutions = new double[1];
+ xSolutions[0] = ( -termB ) / (2.0 * termA);
+ } else if (sqrtTerm < 0.0) {
+ // No solutions
+ }
+
+ double[] distances = null; // The array of signed distances we'll return
+ if (xSolutions == null) {
+ // No solutions -- distances stays null
+ } else {
+ // Unit vector is (cosLambda*cosPhi, cosLambda*sinPhi, sinLambda), so
+ // we can calculate the distances easily...
+ distances = new double[xSolutions.length];
+ for (int i=0; i<distances.length; i++) {
+ // Calculate in a way that remains correct even if we flipped x and y
+ // co-ordinates:
+ distances[i] = (xSolutions[i] - x0) / vx;
+ }
+ }
+
+ return distances;
+ }
+
+ /**
+ * An internal utility routine to find the distance of closest
+ * approach (DOCA) of the line to a point. This is a simple 2D
+ * vector calculation:
+ * * Let the displacement vector from the origin to point be d.
+ * * Decompose d into a component parallel to the line (d_parallel)
+ * and a component perpendicular to the line (d_perp):
+ * d = d_parallel + d_perp
+ * * Take the dot product with the unit vector v along the line to
+ * obtain and use the parallel/perpendicular properties to obtain:
+ * |d.v| = |d_parallel|
+ * * Hence the DOCA, |d_perp|, is given by
+ * |d_perp|^2 = |d|^2 - |d_parallel|^2
+ * = |d|^2 - |d.v|^2
+ */
+ protected double findDOCAToPoint(Hep3Vector point)
+ {
+ // The first line is kind of ugly.
+ Hep3Vector displacement = new BasicHep3Vector(point.x() - origin.x(), point.y() - origin.y(), point.z() - origin.z());
+ Hep3Vector lineDirection = this.getUnitVector();
+ double dotProduct = VecOp.dot(displacement, lineDirection);
+ double doca = Math.sqrt(displacement.magnitudeSquared() - dotProduct*dotProduct);
+ return doca;
+ }
+
+ /**
+ * An internal utility routine to obtain the point of closest approach
+ * (POCA) of the line to a point. The return value is the distance
+ * parameter s, such that getPointAtDistance(s) is the POCA.
+ * The vector calculation is as follows:
+ * Let the origin point be x and the unit vector along the line be v.
+ * Let the point we're trying to find the POCA to be p.
+ * Suppose the POCA is at
+ * x' = x + sv
+ * such that the scalar distance parameter we want is s.
+ * Then the displacement vector from the POCA to p is
+ * d = x' - p
+ * But x' is the POCA, so d is perpendicular to v:
+ * 0 = d.v
+ * = (x' - p).v
+ * = (x + sv - p).v
+ * So
+ * (x-p).v + sv.v = 0
+ * But v is a unit vector, so
+ * s = (p-x).v
+ */
+ protected double findPOCAToPoint(Hep3Vector point)
+ {
+ // Find (p-x)
+ Hep3Vector originToPoint = new BasicHep3Vector(point.x()-origin.x(), point.y() - origin.y(), point.z() - origin.z());
+ Hep3Vector lineDirection = this.getUnitVector();
+ double dotProduct = VecOp.dot(originToPoint, lineDirection);
+ return dotProduct;
+ }
+
+ /**
+ * Internal utility routine to return the unit vector of the
+ * line's direction.
+ */
+ protected Hep3Vector getUnitVector()
+ {
+ Hep3Vector lineDirection = new BasicHep3Vector(cosLambda*cosPhi, cosLambda*sinPhi, sinLambda);
+ return lineDirection;
+ }
}