Example usage for org.apache.commons.math3.geometry.euclidean.threed SphericalCoordinates SphericalCoordinates

Introduction

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

Prototype

public SphericalCoordinates(final double r, final double theta, final double phi) 

Source Link

Document

Build a spherical coordinates transformer from spherical coordinates.

Usage

From source file:fr.amap.lidar.format.jleica.LPointShotExtractor.java

@Override
public Iterator<LShot> iterator() {

    //scan.computeExtremumsAngles();
    //scan.setUpRowToRead(0);

    scan.setReturnInvalidPoint(true);/*  w  ww .  ja  v  a 2  s .c  om*/

    final Iterator<LPoint> pointIterator = scan.iterator();

    Iterator<LShot> it = new Iterator<LShot>() {

        int lastColumnIndex = -1;
        double last = 0.0;
        Vector3d lastVector = new Vector3d(0, 0, 0);

        @Override
        public boolean hasNext() {
            return pointIterator.hasNext();
        }

        @Override
        public LShot next() {

            LShot shot;

            LPoint point = pointIterator.next();

            if (point.valid) {

                double xDirection, yDirection, zDirection;
                if (scan.getHeader().isPointInDoubleFormat()) {
                    xDirection = ((LDoublePoint) point).x;
                    yDirection = ((LDoublePoint) point).y;
                    zDirection = ((LDoublePoint) point).z;
                } else {
                    xDirection = ((LFloatPoint) point).x;
                    yDirection = ((LFloatPoint) point).y;
                    zDirection = ((LFloatPoint) point).z;

                    //System.out.println(xDirection+"\t"+yDirection+"\t"+zDirection);
                }

                Vector3d direction = new Vector3d(xDirection, yDirection, zDirection);
                direction.normalize();

                double range = Math.sqrt(
                        (xDirection * xDirection) + (yDirection * yDirection) + (zDirection * zDirection));

                shot = new LShot(new Point3d(0, 0, 0), direction, new double[] { range });

                //test
                //recalculate shot
                /*double azimutalAngle = (scan.getAzim_min() - ((point.columnIndex - scan.getColIndexAzimMin()) * scan.getAzimutalStepAngle()));
                double elevationAngle = (scan.getElev_min() - ((point.rowIndex - scan.getRowIndexElevMin()) * scan.getElevationStepAngle()));
                        
                SphericalCoordinates sc = new SphericalCoordinates(azimutalAngle, elevationAngle);
                        
                Vector3d testDirection = new Vector3d(sc.toCartesian());
                testDirection.normalize();
                        
                Vec3D theoreticalVector = new Vec3D(testDirection.x, testDirection.y, testDirection.z);
                Vec3D obtainedVector = new Vec3D(direction.x, direction.y, direction.z);
                        
                double angle = Math.acos(Vec3D.dot(theoreticalVector, obtainedVector)/((Vec3D.length(obtainedVector) * Vec3D.length(theoreticalVector))));
                double degreesAngle = Math.toDegrees(angle);*/
                //System.out.println(degreesAngle);

            } else {
                //recalculate shot
                //double azimutalAngle = (scan.getAzim_min() - ((point.columnIndex - scan.getColIndexAzimMin()) * scan.getAzimutalStepAngle()));
                //double elevationAngle = (scan.getElev_min() - ((point.rowIndex - scan.getRowIndexElevMin()) * scan.getElevationStepAngle()));

                SphericalCoordinates sc = new SphericalCoordinates(1,
                        angles[point.rowIndex][point.columnIndex].azimut,
                        angles[point.rowIndex][point.columnIndex].zenith);
                //SphericalCoordinates sc = angles[point.rowIndex][point.columnIndex];
                Vector3d direction = new Vector3d(sc.getCartesian().getX(), sc.getCartesian().getY(),
                        sc.getCartesian().getZ());
                direction.normalize();

                shot = new LShot(new Point3d(0, 0, 0), direction, new double[] {});
                //shot = correctSlope(shot);
            }

            shot.point = point;

            return shot;
        }
    };

    return it;
}

---