processing.core.PMatrix3D Java Examples

/**
 * Start the drawing, to be used in manual mode. This method loads the
 * projection and modelview matrices. It uses the camera member to get the
 * paperScreen location.
 *
 * @param paperScreen
 * @return
 */
@Override
public PGraphicsOpenGL beginDrawOnScreen(PaperScreen paperScreen) {
    PMatrix3D screenPos = paperScreen.getLocation(this.camera);

    this.beginDraw();
    if (this.hasExtrinsics()) {
        screenPos.preApply(getExtrinsics());
    }
    this.graphics.applyMatrix(screenPos);

    // Same origin as in DrawOnPaper
    this.graphics.translate(0, paperScreen.getSize().y);
    this.graphics.scale(1, -1, 1);

    return this.graphics;
}
 

public static void multMatrix(PMatrix3D sum, float value) {
    sum.m00 *= value;
    sum.m01 *= value;
    sum.m02 *= value;
    sum.m03 *= value;

    sum.m10 *= value;
    sum.m11 *= value;
    sum.m12 *= value;
    sum.m13 *= value;

    sum.m20 *= value;
    sum.m21 *= value;
    sum.m22 *= value;
    sum.m23 *= value;

    sum.m30 *= value;
    sum.m31 *= value;
    sum.m32 *= value;
    sum.m33 *= value;
}
 

protected void calibrateDepthToExternalExtr(ArrayList<ExtrinsicSnapshot> snapshots) {
    // Depth -> color  extrinsics
    PMatrix3D kinectExtr = depthCameraDevice.getStereoCalibration().get();

    // color -> depth  extrinsics
    kinectExtr.invert();

    // depth -> tracking
    PMatrix3D kinectCameraExtr = computeKinectCamExtrinsics(snapshots, kinectExtr);

    // // tracking -> depth
    kinectCameraExtr.invert();

    this.kinectCameraExtrinsics.set(kinectCameraExtr);
    saveKinectCameraExtrinsics(kinectCameraExtr);
}
 

/**
 * Save a PMatrix3D to a file. Really simple file format, using saveStrings.
 *
 * @param pa
 * @param mat
 * @param filename
 */
public static void savePMatrix3D(PApplet pa, PMatrix3D mat, String filename) {
    String[] lines = new String[16];
    lines[0] = Float.toString(mat.m00);
    lines[1] = Float.toString(mat.m01);
    lines[2] = Float.toString(mat.m02);
    lines[3] = Float.toString(mat.m03);
    lines[4] = Float.toString(mat.m10);
    lines[5] = Float.toString(mat.m11);
    lines[6] = Float.toString(mat.m12);
    lines[7] = Float.toString(mat.m13);
    lines[8] = Float.toString(mat.m20);
    lines[9] = Float.toString(mat.m21);
    lines[10] = Float.toString(mat.m22);
    lines[11] = Float.toString(mat.m23);
    lines[12] = Float.toString(mat.m30);
    lines[13] = Float.toString(mat.m31);
    lines[14] = Float.toString(mat.m32);
    lines[15] = Float.toString(mat.m33);
    pa.saveStrings(filename, lines);
}
 

private void setXmlTo(XML xml, PMatrix3D matrix) {
    xml.setFloat("m00", matrix.m00);
    xml.setFloat("m01", matrix.m01);
    xml.setFloat("m02", matrix.m02);
    xml.setFloat("m03", matrix.m03);
    xml.setFloat("m10", matrix.m10);
    xml.setFloat("m11", matrix.m11);
    xml.setFloat("m12", matrix.m12);
    xml.setFloat("m13", matrix.m13);
    xml.setFloat("m20", matrix.m20);
    xml.setFloat("m21", matrix.m21);
    xml.setFloat("m22", matrix.m22);
    xml.setFloat("m23", matrix.m23);
    xml.setFloat("m30", matrix.m30);
    xml.setFloat("m31", matrix.m31);
    xml.setFloat("m32", matrix.m32);
    xml.setFloat("m33", matrix.m33);
}
 

public PMatrix3D getProjection(){
  pg_shadowmap.updateProjmodelview();

  // 1) create shadowmap matrix, 
  //    to transform positions from camera-space to the shadowmap-space (light-space)
  PMatrix3D mat_shadow = new PMatrix3D();
  // ndc (shadowmap) -> normalized (shadowmap) 
  //         [-1,+1] -> [0,1]
  mat_shadow.scale(0.5f);
  mat_shadow.translate(1,1,1);

  // model (world) -> modelview (shadowmap) -> ndc (shadowmap)
  mat_shadow.apply(pg_shadowmap.projection);
  
  return mat_shadow;
}
 

/**
 * Get the 3D position. Deprecated.
 *
 * @return the bottom right corner of the markerboard (tracked position).
 */
@Deprecated
public PVector getScreenPos() {
    if (this.isWithoutCamera) {
        PMatrix3D mat = getExtrinsics();
        return new PVector(mat.m03, mat.m13, mat.m23);
    } else if (mainDisplay.hasCamera()) {
        return markerBoard.getBoardLocation(cameraTracking, (ARDisplay) mainDisplay);
    } else {
        System.out.println("Could not find the screen Position for the main display.");
        System.out.println("Looking into secondary displays...");
        for (BaseDisplay display : displays) {
            if (display.hasCamera()) {
                return markerBoard.getBoardLocation(cameraTracking, (ARDisplay) display);
            }
        }
        System.out.println("Could not find where the Screen is...");
        return new PVector();
    }
}
 

private void update(PMatrix3D newPos, int id) {
        PMatrix3D transfo = (PMatrix3D) transfos.get(id);
        fr.inria.papart.multitouch.OneEuroFilter filter[] = filters.get(id);

        if (filter == null) {
            transfo.set(newPos);
        } else {
            try {
                // Rotation
                transfo.m00 = (float) filter[0].filter(newPos.m00);
                transfo.m01 = (float) filter[1].filter(newPos.m01);
                transfo.m02 = (float) filter[2].filter(newPos.m02);
                transfo.m10 = (float) filter[3].filter(newPos.m10);
                transfo.m11 = (float) filter[4].filter(newPos.m11);
                transfo.m12 = (float) filter[5].filter(newPos.m12);
                transfo.m20 = (float) filter[6].filter(newPos.m20);
                transfo.m21 = (float) filter[7].filter(newPos.m21);
                transfo.m22 = (float) filter[8].filter(newPos.m22);

                // Translation
                transfo.m03 = (float) filter[9].filter(newPos.m03);
                transfo.m13 = (float) filter[10].filter(newPos.m13);
                transfo.m23 = (float) filter[11].filter(newPos.m23);
            } catch (Exception e) {
                System.out.println("Filtering error " + e);
            }
        }

//        float pageHeight = this.height;
        float pageHeight = markersFromSVG.getSheetHeight();

        // Invert the scales so that it fits Inkscape's view. 
        transfo.scale(1, -1, 1);
        transfo.translate(0, -pageHeight, 0);
    }
 

void drawJointOrientation(int userId, int jointType, PVector pos, float length)
{
  // draw the joint orientation  
  PMatrix3D  orientation = new PMatrix3D();
  float confidence = context.getJointOrientationSkeleton(userId, jointType, orientation);
  if (confidence < 0.001f) 
    // nothing to draw, orientation data is useless
    return;

  pushMatrix();
  translate(pos.x, pos.y, pos.z);

  // set the local coordsys
  applyMatrix(orientation);

  // coordsys lines are 100mm long
  // x - r
  stroke(255, 0, 0, confidence * 200 + 55);
  line(0, 0, 0, 
  length, 0, 0);
  // y - g
  stroke(0, 255, 0, confidence * 200 + 55);
  line(0, 0, 0, 
  0, length, 0);
  // z - b    
  stroke(0, 0, 255, confidence * 200 + 55);
  line(0, 0, 0, 
  0, 0, length);
  popMatrix();
}
 

public PMatrix3D getTransfoRelativeTo(Camera camera, MarkerBoard board2) {

        PMatrix3D tr1 = getTransfoMat(camera);
        PMatrix3D tr2 = board2.getTransfoMat(camera);

        tr2.apply(tr1);
        return tr2;
    }
 

public Transform asBulletTransform(PMatrix3D mat_p5){
  Matrix4f mat = new Matrix4f();
  mat.setRow(0, mat_p5.m00, mat_p5.m01, mat_p5.m02, mat_p5.m03);
  mat.setRow(1, mat_p5.m10, mat_p5.m11, mat_p5.m12, mat_p5.m13);
  mat.setRow(2, mat_p5.m20, mat_p5.m21, mat_p5.m22, mat_p5.m23);
  mat.setRow(3, mat_p5.m30, mat_p5.m31, mat_p5.m32, mat_p5.m33);
  return new Transform(mat);
}
 

public void scene4(){
  reset();
  
  float mass_mult = 0.33f;
  Vector3f window1_dim = new Vector3f(300, 500, 6);
  Vector3f window2_dim = new Vector3f(400, 300, 6);
  Vector3f window3_dim = new Vector3f(500, 200, 6);
  
  float[] window1_rgb = { 96,160,255};
  float[] window2_rgb = {255, 96, 32};
  float[] window3_rgb = {255,255,255};
 
  BreakableBody window1 = new BreakableBody(this, physics, group_bulletbodies);
  BreakableBody window2 = new BreakableBody(this, physics, group_bulletbodies);
  BreakableBody window3 = new BreakableBody(this, physics, group_bulletbodies);

  
  PMatrix3D win1_mat = new PMatrix3D();
  win1_mat.rotateY(90 * toRadians);
  win1_mat.translate(-(20 + window1_dim.x*0.5f + window1_dim.z), 0, 0);
  window1.initBody(window1_dim, win1_mat, window1_rgb, mass_mult);
  createFitting(win1_mat, window1_dim);
  
  PMatrix3D win2_mat = new PMatrix3D();
  win2_mat.rotateY(90 * toRadians);
  win2_mat.translate(-(20 + window2_dim.x*0.5f + window2_dim.z), 0, 150);
  window2.initBody(window2_dim, win2_mat, window2_rgb, mass_mult);
  createFitting(win2_mat, window2_dim, true);
  
  
  PMatrix3D win3_mat = new PMatrix3D();
  win3_mat.rotateY(90 * toRadians);
  win3_mat.translate(-(20 + window3_dim.x*0.5f + window3_dim.z), 0, -150);
  window3.initBody(window3_dim, win3_mat, window3_rgb, mass_mult);
  createFitting(win3_mat, window3_dim, true);
}
 

public Transform asBulletTransform(PMatrix3D mat_p5){
  Matrix4f mat = new Matrix4f();
  mat.setRow(0, mat_p5.m00, mat_p5.m01, mat_p5.m02, mat_p5.m03);
  mat.setRow(1, mat_p5.m10, mat_p5.m11, mat_p5.m12, mat_p5.m13);
  mat.setRow(2, mat_p5.m20, mat_p5.m21, mat_p5.m22, mat_p5.m23);
  mat.setRow(3, mat_p5.m30, mat_p5.m31, mat_p5.m32, mat_p5.m33);
  return new Transform(mat);
}
 

public PMatrix3D getHardwareExtrinsics() {
    RealSense.extrinsics extrinsics = grabber.getRealSenseDevice().get_extrinsics(RealSense.color, RealSense.depth);
    FloatBuffer fb = extrinsics.position(0).asByteBuffer().asFloatBuffer();
    return new PMatrix3D(
            fb.get(0), fb.get(3), fb.get(6), -fb.get(9) * 1000f,
            fb.get(1), fb.get(4), fb.get(7), fb.get(10) * 1000f,
            fb.get(2), fb.get(5), fb.get(8), fb.get(11) * 1000f,
            0, 0, 0, 1);
}
 

public void scene1(){
  reset();
  
  float mass_mult = 0.33f;
  Vector3f window1_dim = new Vector3f(600, 300, 6);
  Vector3f window2_dim = new Vector3f(400, 500, 6);
  Vector3f window3_dim = new Vector3f(300, 600, 6);
  
  float[] window1_rgb = { 96,160,255};
  float[] window2_rgb = {255, 96, 32};
  float[] window3_rgb = {255,255,255};
 
  BreakableBody window1 = new BreakableBody(this, physics, group_bulletbodies);
  BreakableBody window2 = new BreakableBody(this, physics, group_bulletbodies);
  BreakableBody window3 = new BreakableBody(this, physics, group_bulletbodies);

  
  PMatrix3D win1_mat = new PMatrix3D();
  win1_mat.rotateX(90 * toRadians);
  win1_mat.translate(0, 20 + window1_dim.y*0.5f, 0);
  win1_mat.rotateY(-10 * toRadians);
  window1.initBody(window1_dim, win1_mat, window1_rgb, mass_mult);
  createFitting(win1_mat, window1_dim);
  
  PMatrix3D win2_mat = new PMatrix3D();
  win2_mat.rotateX(90 * toRadians);
  win2_mat.translate(0, 20 + window2_dim.y*0.5f, +150);
  window2.initBody(window2_dim, win2_mat, window2_rgb, mass_mult);
  createFitting(win2_mat, window2_dim);
  
  PMatrix3D win3_mat = new PMatrix3D();
  win3_mat.rotateX(90 * toRadians);
  win3_mat.translate(110, 20 + window3_dim.y*0.5f, -150);
  win3_mat.rotateY(10 * toRadians);
  window3.initBody(window3_dim, win3_mat, window3_rgb, mass_mult);
  createFitting(win3_mat, window3_dim);
}
 

public void scene1(){
  reset();
  
  float mass_mult = 0.33f;
  Vector3f window1_dim = new Vector3f(600, 300, 6);
  Vector3f window2_dim = new Vector3f(400, 500, 6);
  Vector3f window3_dim = new Vector3f(300, 600, 6);
  
  float[] window1_rgb = { 96,160,255};
  float[] window2_rgb = {255, 96, 32};
  float[] window3_rgb = {255,255,255};
 
  BreakableBody window1 = new BreakableBody(this, physics, group_bulletbodies);
  BreakableBody window2 = new BreakableBody(this, physics, group_bulletbodies);
  BreakableBody window3 = new BreakableBody(this, physics, group_bulletbodies);

  
  PMatrix3D win1_mat = new PMatrix3D();
  win1_mat.rotateX(90 * toRadians);
  win1_mat.translate(0, 20 + window1_dim.y*0.5f, 0);
  win1_mat.rotateY(-10 * toRadians);
  window1.initBody(window1_dim, win1_mat, window1_rgb, mass_mult);
  createFitting(win1_mat, window1_dim);
  
  PMatrix3D win2_mat = new PMatrix3D();
  win2_mat.rotateX(90 * toRadians);
  win2_mat.translate(0, 20 + window2_dim.y*0.5f, +150);
  window2.initBody(window2_dim, win2_mat, window2_rgb, mass_mult);
  createFitting(win2_mat, window2_dim);
  
  PMatrix3D win3_mat = new PMatrix3D();
  win3_mat.rotateX(90 * toRadians);
  win3_mat.translate(110, 20 + window3_dim.y*0.5f, -150);
  win3_mat.rotateY(10 * toRadians);
  window3.initBody(window3_dim, win3_mat, window3_rgb, mass_mult);
  createFitting(win3_mat, window3_dim);
}
 

private PMatrix3D compute3DPos(double[] corners, Camera camera) {

        //  double[] srcCorners = {0, 0,  w, 0,  w, h,  0, h};
        botLeft.set((float) corners[0], (float) corners[1]);
        botRight.set((float) corners[2], (float) corners[3]);
        topRight.set((float) corners[4], (float) corners[5]);
        topLeft.set((float) corners[6], (float) corners[7]);

        // check image bounds...
        if (botLeft.x < 0 || botRight.x < 0 || topLeft.x < 0 || topRight.x < 0
                || botLeft.x > camera.width() || botRight.x > camera.width() || topLeft.x > camera.width() || topRight.x > camera.width()
                || botLeft.y < 0 || botRight.y < 0 || topLeft.y < 0 || topRight.y < 0
                || botLeft.y > camera.height() || botRight.y > camera.height() || topLeft.y > camera.height() || topRight.y > camera.height()) {
            return null;
        }

//        imagePoints[0] = botLeft;
//        imagePoints[1] = botRight;
//        imagePoints[2] = topRight;
//        imagePoints[3] = topLeft;
        imagePoints[0] = topLeft;
        imagePoints[1] = topRight;
        imagePoints[2] = botRight;
        imagePoints[3] = botLeft;

//      objectPoints[0] = new PVector(0, 0, 0);
//      objectPoints[1] = new PVector(width, 0, 0);
//      objectPoints[2] = new PVector(width, height, 0);
//      objectPoints[3] = new PVector(0, height, 0);
        ProjectiveDeviceP pdp = camera.getProjectiveDevice();
        return pdp.estimateOrientation(objectPoints, imagePoints);

    }
 

public static Object invoke(final Object arg0, final double angle)
{
    final PMatrix3D mat = (PMatrix3D)arg0;

    mat.rotateZ((float)angle);

    return mat;
}
 

@Override
    public void setup() {
        try {

            if (tableTest == null) {
//                tableTest = new TableTest();
//                tableTest.setDrawing(false);
            }
            setDrawingFilter(0);
            setTrackingFilter(0, 0);
//            planeProjCalib = new PlaneAndProjectionCalibration();
            savedImages = new IplImage[nbScreenPoints];
            savedLocations = new PMatrix3D[nbScreenPoints];
            savedColors = new int[nbScreenPoints * 2][nbColors];

            this.seeThrough = true;
//            initButtons();
            initScreenPoints();

            initColorTrackers();
            // GREEN circle 
//                 fill(0, 255, 0);
//        rect(79.8f, 123.8f, 15f, 15f);
            if (papart.isTouchInitialized()) {
                depthTouchInput = (DepthTouchInput) papart.getTouchInput();
                depthTouchInput.useRawDepth();
                depthCameraDevice = papart.getDepthCameraDevice();
                useTouch = true;
            }
            
        } catch (Exception e) {
            e.printStackTrace();
        }
        startCalib();
    }
 

@Override
public void setCameraMatrix(Matrix4f matrix, Vector3f lookAt, Vector3f eye, Vector3f up)
{
	PMatrix3D pMatrix = ProcessingUtil.toPMatrix(matrix);
	
	graphics.camera.set(pMatrix);
}
 

public static Object invoke(final Object arg0, final double x, final double y,
    final double z)
{
    final PMatrix3D mat = (PMatrix3D)arg0;

    mat.translate((float)x, (float)y, (float)z);

    return mat;
}
 

public static Object invoke(final Object arg0, final double angle)
{
    final PMatrix3D mat = (PMatrix3D)arg0;

    mat.rotateX((float)angle);

    return mat;
}
 

public DwSkyLight(DwPixelFlow context, DwSceneDisplay scene_display, PMatrix3D mat_scene_bounds){

    this.scene_display = scene_display;
 
    int shadowmap_wh = 1024; // default value, can be resized anytime
    
    DwShadowMap sun_shadowmap = new DwShadowMap(context, shadowmap_wh, scene_display, mat_scene_bounds);
    DwShadowMap sky_shadowmap = new DwShadowMap(context, shadowmap_wh, scene_display, mat_scene_bounds);
    
    geom = new DwScreenSpaceGeometryBuffer(context, scene_display);
    
    sun = new DwSkyLightShader(context, scene_display, geom, sun_shadowmap);
    sky = new DwSkyLightShader(context, scene_display, geom, sky_shadowmap);
    
    // composition of sky and sun for rendering
    renderer = new DwSkyLightRenderer(context, scene_display, geom, sky, sun);
    
    // default starting parameters for sky-light
    sky.param.solar_azimuth  = 0;
    sky.param.solar_zenith   = 0;
    sky.param.sample_focus   = 1;
    sky.param.rgb = new float[]{1,1,1};
    
    // default starting parameters for sun-light
    sun.param.solar_azimuth  = 45;
    sun.param.solar_zenith   = 80;
    sun.param.sample_focus   = 0.05f;
    sun.param.rgb = new float[]{1,1,1};
  }
 

public boolean isIdentity(PMatrix3D mat) {
    PMatrix3D identity = new PMatrix3D();
    identity.reset();
    float[] identityArray = new float[16];
    float[] matArray = new float[16];
    identity.get(identityArray);
    mat.get(matArray);

    return Arrays.equals(identityArray, matArray);
}
 

private PlaneCalibration getTouchPlane(PMatrix3D paperViewedByDepth) {
        PlaneCalibration planeCalib
                = PlaneCalibration.CreatePlaneCalibrationFrom(paperViewedByDepth,
                        //app.getLocation(),
                        new PVector(100, 100));
        planeCalib.flipNormal();

        // Y shift here. 
//        planeCalib.moveAlongNormal(zShift);
        return planeCalib;
    }
 

/**
 * Get the 3D location of the PaperScreen. This takes into account the
 * setLocation() calls.
 *
 * @return
 */
public PMatrix3D getLocation() {
    if (this.useManualLocation) {
        return this.manualLocation;
    }
    return this.getLocation(cameraTracking);
}
 

private void tryLoadExtrinsics() {
    PMatrix3D extrinsics = loadCalibration(cameraProjExtrinsics);
    if (extrinsics == null) {
        System.out.println("loading default extrinsics. Could not find " + cameraProjExtrinsics + " .");
    } else {
        arDisplay.setExtrinsics(extrinsics);
    }
}
 

public static ProjectiveDeviceP createDevice(float fx, float fy, float cx, float cy, int w, int h,
            float k1, float k2, float k3, float k4, float k5) {
        ProjectiveDeviceP p = new ProjectiveDeviceP();
        // Do not update the handle distorsions ?
//        p.handleDistorsion = false;
        p.w = w;
        p.h = h;
        p.intrinsics = new PMatrix3D(fx, 0, cx, 0,
                0, fy, cy, 0,
                0, 0, 0, 0,
                0, 0, 0, 0);
        p.updateFromIntrinsics();

        p.device = new ProjectiveDevice("device");
        ProjectiveDevice d = p.device;

        d.cameraMatrix = CvMat.create(3, 3);

        d.cameraMatrix.put(fx, 0.0, cx,
                0.0, fy, cy,
                0.0, 0.0, 1);

        d.imageWidth = w;
        d.imageHeight = h;
        d.distortionCoeffs = CvMat.create(1, 5);
        d.distortionCoeffs.put(0, k1);
        d.distortionCoeffs.put(1, k2);
        d.distortionCoeffs.put(2, k3);
        d.distortionCoeffs.put(3, k4);
        d.distortionCoeffs.put(4, k5);
        p.handleDistorsion = true;

        return p;
    }
 

public static PMatrix3D createDummyValues() {
    float[] dummyValues = {
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16};
    PMatrix3D mat = new PMatrix3D();
    mat.set(dummyValues);
    return mat;
}
 

public ExtrinsicSnapshot(PMatrix3D cameraPaperCalibration,
        PMatrix3D projectorPaperCalibration,
        PMatrix3D kinectPaperCalibration) {

    if (cameraPaperCalibration != null) {
        mainCameraPaper = cameraPaperCalibration.get();
    }
    if (projectorPaperCalibration != null) {
        projectorPaper = projectorPaperCalibration.get();
    }
    if (kinectPaperCalibration != null) {
        kinectPaper = kinectPaperCalibration.get();
    }
}