3D Misc

From EOJ Wiki
Jump to navigation Jump to search
 
public class Geom3D {
    // distance from point (x, y, z) to plane aX + bY + cZ + d = 0
    public static double ptPlaneDist(double x, double y, double z,
                                     double a, double b, double c, double d) {
        return Math.abs(a * x + b * y + c * z + d) / Math.sqrt(a * a + b * b + c * c);
    }

    // distance between parallel planes aX + bY + cZ + d1 = 0 and
    // aX + bY + cZ + d2 = 0
    public static double planePlaneDist(double a, double b, double c,
                                        double d1, double d2) {
        return Math.abs(d1 - d2) / Math.sqrt(a * a + b * b + c * c);
    }

    // distance from point (px, py, pz) to line (x1, y1, z1)-(x2, y2, z2)
    // (or ray, or segment; in the case of the ray, the endpoint is the
    // first point)
    public static final int LINE = 0;
    public static final int SEGMENT = 1;
    public static final int RAY = 2;

    public static double ptLineDistSq(double x1, double y1, double z1,
                                      double x2, double y2, double z2,
                                      double px, double py, double pz,
                                      int type) {
        double pd2 = (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2);
        double x, y, z;
        if (pd2 == 0) {
            x = x1;
            y = y1;
            z = z1;
        } else {
            double u = ((px - x1) * (x2 - x1) + (py - y1) * (y2 - y1) +
                        (pz - z1) * (z2 - z1)) / pd2;
            x = x1 + u * (x2 - x1);
            y = y1 + u * (y2 - y1);
            z = z1 + u * (z2 - z1);
            if (type != LINE && u < 0) {
                x = x1;
                y = y1;
                z = z1;
            }
            if (type == SEGMENT && u > 1.0) {
                x = x2;
                y = y2;
                z = z2;
            }
        }
        return (x - px) * (x - px) + (y - py) * (y - py) + (z - pz) * (z - pz);
    }

    public static double ptLineDist(double x1, double y1, double z1,
                                    double x2, double y2, double z2,
                                    double px, double py, double pz,
                                    int type) {
        return Math.sqrt(ptLineDistSq(x1, y1, z1, x2, y2, z2, px, py, pz, type));
    }
}