Example usage for org.apache.commons.math3.geometry.euclidean.threed Rotation applyTo

Introduction

In this page you can find the example usage for org.apache.commons.math3.geometry.euclidean.threed Rotation applyTo.

Prototype

public void applyTo(final double[] in, final double[] out) 

Source Link

Document

Apply the rotation to a vector stored in an array.

Usage

From source file:com.rvantwisk.cnctools.controls.opengl.ArrowsActor.java

private void addArrow(double[][] arrow, double[] loc, double rA, final Vector3D p1, final Vector3D p2) {

    try {//from w  w  w  .  j  a  va  2  s .  co m
        double angleZ = Point.angleBetween2Lines(new Point(p1.getX(), p1.getY()),
                new Point(p2.getX(), p2.getY()), new Point(0.0, 0.0), new Point(0.0, 1.0));
        //        double angleX = Point.angleBetween2Lines(new Point(lastY, lastZ), new Point(rY, rZ), new Point(0.0, 0.0), new Point(0.0, 1.0));

        double dx = p1.getX() - p2.getX();
        double dy = p1.getY() - p2.getY();
        double d = Math.sqrt(dx * dx + dy * dy);

        double angle;
        Rotation myRotation;
        if (d != 0.0) {
            angle = Point.angleBetween2Lines(new Point(0.0, 0.0), new Point(1.0, 0.0), new Point(0.0, 0.0),
                    new Point(d, p1.getZ() - p2.getZ()));
            myRotation = new Rotation(new Vector3D(1, 0, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0));
        } else if ((p1.getZ() - p2.getZ()) < 0.0) {
            angle = (90.0 / 360.0 * Math.PI * 2.0);
            myRotation = new Rotation(new Vector3D(0, 1, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0));
        } else {
            angle = (-90.0 / 360.0 * Math.PI * 2.0);
            myRotation = new Rotation(new Vector3D(0, 1, 0.0), angle + (0.0 / 360.0 * Math.PI * 2.0));
        }
        Rotation myRotationZ = new Rotation(new Vector3D(0, 0, 1.0), angleZ + (-90.0 / 360.0 * Math.PI * 2.0));

        Rotation myRotationA = new Rotation(new Vector3D(1.0, 0.0, 0.0), (rA / 360.0 * Math.PI * 2.0));

        double[] out = new double[3];
        double[] out2 = new double[3];
        for (double[] v : arrow) {
            myRotationZ.applyTo(v, out);
            myRotation.applyTo(out, out2);

            out2[0] = out2[0] + loc[0];
            out2[1] = out2[1] + loc[1];
            out2[2] = out2[2] + loc[2];

            myRotationA.applyTo(out2, out2);

            data.add((float) (out2[0] + 0.0));
            data.add((float) (out2[1] + 0.0));
            data.add((float) (out2[2] + 0.0));
            setMotionColor(machine.getMotionMode());
        }

    } catch (Exception e) {
        // If for some reason we get a exception, we just don't add the arrow
        logger.warn("Add arrow : This should normally not happen, please report back.", e);
    }

}

---