Java floor floorPlaneAndHorizToPlanView(final int[][] frameCells, final short frame[], final int h)

Description

floor Plane And Horiz To Plan View

License

Open Source License

Declaration

public static byte[][] floorPlaneAndHorizToPlanView(final int[][] frameCells, final short frame[],
            final int h) 

Method Source Code

//package com.java2s;
//License from project: Open Source License 

public class Main {
    final static int width = 320;
    final static int height = 240;
    final static int camFOVx = 58;
    public final static int maxDepthFPTV = 3500;

    public static byte[][] floorPlaneAndHorizToPlanView(final int[][] frameCells, final short frame[],
            final int h) {
        final int w = (int) (Math.sin((camFOVx / 2) * Math.PI / 180) * h) * 2;
        final int cwidth = frameCells.length;
        final int cheight = frameCells[0].length;
        final double angle = (double) camFOVx / 2 * Math.PI / 180; // 0.392699082; // 29 deg in radians from ctr, or half included view angle

        byte[][] result = new byte[w][h];

        final int xdctr = cwidth / 2;

        for (int y = 0; y < cheight; y++) {
            for (int x = 0; x < cwidth; x++) {
                int d = frameCells[x][y]; // depth

                byte b = 0;
                if ((d & 0xf0000) >> 16 == 1) {
                    d = (d & 0xffff);
                    b = 0b01;/* w  ww .  ja  v  a  2  s  . co  m*/
                }

                int ry = (int) ((double) d / (double) maxDepthFPTV * (double) h);
                double xdratio = (double) (x - xdctr) / (double) xdctr;
                int rx = (w / 2) - ((int) (Math.tan(angle) * (double) ry * xdratio));

                if (ry < h && ry >= 0 && rx >= 0 && rx < w && b != 0) {
                    result[rx][h - ry - 1] = b;
                }

            }
        }

        // now overlay horiz
        for (int y = height / 2 - 0; y <= height / 2 + 0; y++) {
            for (int x = 0; x < width; x += 1) {

                int d = frame[y * width + x]; // -cameraSetBack;
                int ry = (int) ((double) d / (double) maxDepthFPTV * (double) h);
                double xdratio = (double) (x * (double) cwidth / width - xdctr) / (double) xdctr;
                int rx = (w / 2) - ((int) (Math.tan(angle) * (double) ry * xdratio));

                if (ry < h && ry >= 0 && rx >= 0 && rx < w) {
                    result[rx][h - ry - 1] = 0b11;
                }
            }
        }

        result[w / 2][h - 1] = 0;
        return result;
    }
}

Related

1. floormod(float a, float b)
2. floorMod(long x, long y)
3. floorMod(long x, long y)
4. floorModExt(int dividend, int divisor)
5. floorModulo(int value, int divisor)
6. floorPlaneToPlanView(int[][] frameCells, final int h)
7. floorPositive(float value)
8. floorPot(float value)
9. floorPOT(int n)