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

Introduction

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

Prototype

public Vector3D getCartesian() 

Source Link

Document

Get the Cartesian 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  . j  a v  a  2 s. co m*/

    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;
}

---