Java Code Examples for processing.core.PMatrix3D#translate()

/**
     * Get a plane from a 3D matrix. Here the size is not that important, might
     * be removed.
     *
     * @param mat
     * @param size
     * @return
     */
    public static PlaneCalibration CreatePlaneCalibrationFrom(PMatrix3D mat, PVector size) {
        PMatrix3D matrix = mat.get();
        PlaneCreator planeCreator = new PlaneCreator();

        planeCreator.addPoint(new Vec3D(matrix.m03, matrix.m13, matrix.m23));
        matrix.translate(size.x, 0, 0);
        planeCreator.addPoint(new Vec3D(matrix.m03, matrix.m13, matrix.m23));
        matrix.translate(0, size.y, 0);
        planeCreator.addPoint(new Vec3D(matrix.m03, matrix.m13, matrix.m23));

        planeCreator.setHeight(DEFAULT_PLANE_HEIGHT);
        assert (planeCreator.isComputed());
        PlaneCalibration planeCalibration = planeCreator.getPlaneCalibration();

        planeCalibration.flipNormal();
//        planeCalibration.moveAlongNormal(DEFAULT_PLANE_SHIFT);
        assert (planeCalibration.isValid());
        return planeCalibration;
    }
 

/**
 * Return the x,y positions of a 3D location projected onto a given
 * reference. The Z axis is the angle (in radians) given by the rotation of
 * the positionToFind.
 *
 * @param positionToFind
 * @param reference
 * @return
 */
public PVector projectPositionTo2D(PMatrix3D positionToFind,
        PMatrix3D reference,
        float referenceHeight) {
    PMatrix3D referenceInv = reference.get();
    referenceInv.invert();
    PMatrix3D relative = positionToFind.get();
    relative.preApply(referenceInv);

    PMatrix3D positionToFind2 = positionToFind.get();
    positionToFind2.translate(100, 0, 0);
    PMatrix3D relative2 = positionToFind2.get();
    relative2.preApply(referenceInv);

    PVector out = new PVector();
    float x = relative.m03 - relative2.m03;
    float y = relative.m13 - relative2.m13;
    out.z = PApplet.atan2(x, y);

    out.x = relative.m03;
    out.y = referenceHeight - relative.m13;

    return out;
}
 

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

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 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 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 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 applyTransformationTo(PMatrix3D matrix) {
    matrix.translate(sceneTranslate.x, sceneTranslate.y, sceneTranslate.z);
    matrix.rotateX(sceneRotateX);
    matrix.rotateY(sceneRotateY);
    matrix.rotateZ(sceneRotateZ);
    matrix.scale(sceneScale);
}
 

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 void scene2(){
    reset();
    
    float mass_mult = 1;
    Vector3f window1_dim = new Vector3f(200, 500, 3);
    Vector3f window2_dim = new Vector3f(200, 500, 3);
    Vector3f window3_dim = new Vector3f(200, 500, 3);
    
    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.rotateZ(random(-20,20) * toRadians);
    win1_mat.rotateX(random(-10,10) * toRadians);

    win1_mat.translate(0, 200 + 20 + window1_dim.y*0.5f, 0);
    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.rotateZ(random(-10,10) * toRadians);
    win2_mat.rotateX(random(-10,10) * toRadians);
    win2_mat.translate(0, 50 + 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.rotateZ(random(-10,10) * toRadians);
    win3_mat.rotateX(random(-10,10) * toRadians);
    win3_mat.translate(0, 100 + 20 + window3_dim.y*0.5f, -150);
    window3.initBody(window3_dim, win3_mat, window3_rgb, mass_mult);
//    createFitting(win3_mat, window3_dim);
  }
 

private void computeCorners() {

        PMatrix3D pos = null;

        if (useManualConrers) {
            return;
        }

        if (usePaperLocation) {
            pos = paperScreen.getLocation();
        }

        if (useBoardLocation) {
            pos = board.getTransfoMat(camera).get();
        }

        if (pos == null) {
            throw new RuntimeException("ERROR in TrackedView, report this.");
        }

        PMatrix3D tmp = new PMatrix3D();

        tmp.apply(pos);

        for (int i = 0; i < 4; i++) {
            corner3DPos[i] = new PVector();
        }
        if (isYUp) {

            // bottom left
            tmp.translate(topLeftCorner.x, topLeftCorner.y);
            corner3DPos[0].x = tmp.m03;
            corner3DPos[0].y = tmp.m13;
            corner3DPos[0].z = tmp.m23;

            // bottom right
            tmp.translate(captureSizeMM.x, 0);
            corner3DPos[1].x = tmp.m03;
            corner3DPos[1].y = tmp.m13;
            corner3DPos[1].z = tmp.m23;

            // top right
            tmp.translate(0, -captureSizeMM.y, 0);
            corner3DPos[2].x = tmp.m03;
            corner3DPos[2].y = tmp.m13;
            corner3DPos[2].z = tmp.m23;

            // top left
            tmp.translate(-captureSizeMM.x, 0, 0);
            corner3DPos[3].x = tmp.m03;
            corner3DPos[3].y = tmp.m13;
            corner3DPos[3].z = tmp.m23;

        } else {

            // TODO: use BottowLeftCorner here ?!! 
            // top left
            tmp.translate(topLeftCorner.x, paperScreen.getDrawingSize().y - topLeftCorner.y);
            corner3DPos[3].x = tmp.m03;
            corner3DPos[3].y = tmp.m13;
            corner3DPos[3].z = tmp.m23;

            // top right
            tmp.translate(captureSizeMM.x, 0);
            corner3DPos[2].x = tmp.m03;
            corner3DPos[2].y = tmp.m13;
            corner3DPos[2].z = tmp.m23;

            // bottom right
            tmp.translate(0, -captureSizeMM.y, 0);
            corner3DPos[1].x = tmp.m03;
            corner3DPos[1].y = tmp.m13;
            corner3DPos[1].z = tmp.m23;

            // bottom left
            tmp.translate(-captureSizeMM.x, 0, 0);
            corner3DPos[0].x = tmp.m03;
            corner3DPos[0].y = tmp.m13;
            corner3DPos[0].z = tmp.m23;
        }

        screenPixelCoordinates.clear();
        for (int i = 0; i < 4; i++) {
            screenPixelCoordinates.add(camera.pdp.worldToPixel(corner3DPos[i], true));
        }
        cornersSet = true;
    }
 

/**
 * Init VR rendering. The VR rendering creates a 3D "screen". It is used to
 * create 3D pop-up effects.
 *
 * @param cam Rendering origin.
 * @param userPos Position of the user, relative to the PaperScreen
 * @param nearPlane Close disance for OpengL in millimeters.
 * @param farPlane Far distance for OpenGL in millimeters.
 * @param isAnaglyph Use Anaglyph.
 * @param isLeft When analygph is it left or right, ignored otherwise.
 */
public void initDraw(Camera cam, PVector userPos, float nearPlane, float farPlane, boolean isAnaglyph, boolean isLeft) {

    PGraphicsOpenGL graphics = getGraphics();

    if (initPosM == null) {
        this.isOpenGL = true;
        // Transformation  Camera -> Marker

        initPosM = this.getLocation(cam);

        initPosM.translate(this.getRenderingSizeX() / 2, this.getRenderingSizeY() / 2);
        // All is relative to the paper's center. not the corner. 
        initPosM.scale(-1, 1, 1);

    }

    // get the current transformation... 
    PMatrix3D newPos = this.getLocation(cam);

    newPos.translate(this.getRenderingSizeX() / 2, this.getRenderingSizeY() / 2);
    newPos.scale(-1, 1, 1);

    newPos.invert();
    newPos.apply(initPosM);

    PVector user = new PVector();

    if (isAnaglyph && isLeft) {
        userPos.add(-halfEyeDist * 2, 0, 0);
    }
    newPos.mult(userPos, user);
    PVector paperCameraPos = user;

    // Camera must look perpendicular to the screen. 
    graphics.camera(paperCameraPos.x, paperCameraPos.y, paperCameraPos.z,
            paperCameraPos.x, paperCameraPos.y, 0,
            0, 1, 0);

    // http://www.gamedev.net/topic/597564-view-and-projection-matrices-for-vr-window-using-head-tracking/
    float nearFactor = nearPlane / paperCameraPos.z;

    float left = nearFactor * (-drawingSize.x / 2f - paperCameraPos.x);
    float right = nearFactor * (drawingSize.x / 2f - paperCameraPos.x);
    float top = nearFactor * (drawingSize.y / 2f - paperCameraPos.y);
    float bottom = nearFactor * (-drawingSize.y / 2f - paperCameraPos.y);

    graphics.frustum(left, right, bottom, top, nearPlane, farPlane);
    graphics.projection.m11 = -graphics.projection.m11;

    // No detection?
    PMatrix3D transformation = this.getLocation(cam);
    if (transformation.m03 == 0 && transformation.m13 == 0 && transformation.m23 == 0) {
        resetPos();
    }
}
 

public PMatrix3D getUnitSphereMatrix(){
  PMatrix3D mat = new PMatrix3D();
  mat.scale(1.0f/rad);
  mat.translate(-pos[0], -pos[1], -pos[2]);
  return mat;
}
 

public void scene3(){
    reset();
    
    float mass_mult = 1;
    Vector3f window1_dim = new Vector3f(600, 200, 8);
    Vector3f window2_dim = new Vector3f(200, 600, 8);
    Vector3f window3_dim = new Vector3f(400, 400, 8);
    
    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.rotateZ(random(-45,45) * toRadians);
//    win1_mat.rotateX(random(-5,5) * toRadians);
    win1_mat.translate(0, 0, 330);
    window1.initBody(window1_dim, win1_mat, window1_rgb, mass_mult);
    
//    createFitting(win1_mat, window1_dim);
    
    PMatrix3D win2_mat = new PMatrix3D();
    win2_mat.rotateZ(random(-90,45) * toRadians);
//    win2_mat.rotateX(random(-5,5) * toRadians);
    win2_mat.translate(0, 0, 460);
    window2.initBody(window2_dim, win2_mat, window2_rgb, mass_mult);
//    createFitting(win2_mat, window2_dim);
    
    PMatrix3D win3_mat = new PMatrix3D();
    win1_mat.rotateZ(random(-45,45) * toRadians);
    win3_mat.rotateX(random(-5,5) * toRadians);
    win3_mat.translate(0, 0, 200);
    window3.initBody(window3_dim, win3_mat, window3_rgb, mass_mult);
    createFitting(win3_mat, window3_dim);
    
//    window1.body.setVelocity(new Vector3f(0,0,-100));
//    window2.body.setVelocity(new Vector3f(0,0,-100));
  }
 

public void createFractureShape(){

    reset();

    float pos_z = 20f;

    int   numx = 15;
    int   numy = 5;
    int   numz = 10;
    
    float dim = 20;
    
    float boundsx = numx * dim;
    float boundsy = numy * dim;
    // float boundsz = numz * dim;
    
    float mass = dim*dim*dim;
    
    float tx = -boundsx * 0.5f;
    float ty = -boundsy * 0.5f;
    float tz = pos_z  + 0;
    
    colorMode(HSB, 360, 100,100);

    for(int z = 0; z < numz; z++){
      for(int y = 0; y < numy; y++){
        for(int x = 0; x < numx; x++){
          float cx = dim *0.5f + x * dim;
          float cy = dim *0.5f + y * dim;
          float cz = dim *0.5f + z * dim;
          
          BObject body = new BBox(this, mass, dim, dim, dim);

          // setup initial body transform-matrix
          PMatrix3D mat_p5 = new PMatrix3D();
          mat_p5.translate(tx, ty, tz);
          mat_p5.translate(cx, cy, cz);

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

          Transform transform = new Transform(mat);
          
          // rigid-body properties
          body.rigidBody.setWorldTransform(transform);
          body.rigidBody.setRestitution(.1f);
          body.rigidBody.setFriction(0.85f);
          body.rigidBody.setDamping(0.2f, 0.2f);
          
          body.displayShape = createShape(BOX, dim, dim, dim);
          body.displayShape.setStroke(false);
          body.displayShape.setFill(true);
          
//          float r = random(80, 130);
          float r = random(220, 270);
          float g = random(20,70);
          float b = 100;
          body.displayShape.setFill(color(r,g,b));
          
          physics.addBody(body);
          group_bulletbodies.addChild(body.displayShape);
        }
      }
    }
    colorMode(RGB, 255, 255,255);
    
  }
 

public void scene3(){
    reset();
    
    float mass_mult = 1;
    Vector3f window1_dim = new Vector3f(600, 200, 8);
    Vector3f window2_dim = new Vector3f(200, 600, 8);
    Vector3f window3_dim = new Vector3f(400, 400, 8);
    
    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.rotateZ(random(-45,45) * toRadians);
//    win1_mat.rotateX(random(-5,5) * toRadians);
    win1_mat.translate(0, 0, 330);
    window1.initBody(window1_dim, win1_mat, window1_rgb, mass_mult);
    
//    createFitting(win1_mat, window1_dim);
    
    PMatrix3D win2_mat = new PMatrix3D();
    win2_mat.rotateZ(random(-90,45) * toRadians);
//    win2_mat.rotateX(random(-5,5) * toRadians);
    win2_mat.translate(0, 0, 460);
    window2.initBody(window2_dim, win2_mat, window2_rgb, mass_mult);
//    createFitting(win2_mat, window2_dim);
    
    PMatrix3D win3_mat = new PMatrix3D();
    win1_mat.rotateZ(random(-45,45) * toRadians);
    win3_mat.rotateX(random(-5,5) * toRadians);
    win3_mat.translate(0, 0, 200);
    window3.initBody(window3_dim, win3_mat, window3_rgb, mass_mult);
    createFitting(win3_mat, window3_dim);
    
//    window1.body.setVelocity(new Vector3f(0,0,-100));
//    window2.body.setVelocity(new Vector3f(0,0,-100));
  }
 

public void scene2(){
    reset();
    
    float mass_mult = 1;
    Vector3f window1_dim = new Vector3f(200, 500, 3);
    Vector3f window2_dim = new Vector3f(200, 500, 3);
    Vector3f window3_dim = new Vector3f(200, 500, 3);
    
    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.rotateZ(random(-20,20) * toRadians);
    win1_mat.rotateX(random(-10,10) * toRadians);

    win1_mat.translate(0, 200 + 20 + window1_dim.y*0.5f, 0);
    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.rotateZ(random(-10,10) * toRadians);
    win2_mat.rotateX(random(-10,10) * toRadians);
    win2_mat.translate(0, 50 + 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.rotateZ(random(-10,10) * toRadians);
    win3_mat.rotateX(random(-10,10) * toRadians);
    win3_mat.translate(0, 100 + 20 + window3_dim.y*0.5f, -150);
    window3.initBody(window3_dim, win3_mat, window3_rgb, mass_mult);
//    createFitting(win3_mat, window3_dim);
  }
 

public void scene0(){
    reset();
    
    Vector3f window1_dim = new Vector3f(400, 400, 6);
    Vector3f window2_dim = new Vector3f(400, 400, 6);
    Vector3f window3_dim = new Vector3f(400, 400, 6);
//    Vector3f window4_dim = new Vector3f(150, 400, 6);

    float[] window1_rgb = { 96,160,255};
    float[] window2_rgb = {255, 96, 32};
    float[] window3_rgb = {255,255,255};
//    float[] window4_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);
//    BreakableBody window4 = new BreakableBody(this, physics, group_bulletbodies);

    
    
    PMatrix3D win1_mat = new PMatrix3D();
    win1_mat.translate(0, 0, 125);
    window1.initBody(window1_dim, win1_mat, window1_rgb);
    createFitting(win1_mat, window1_dim);
    
    PMatrix3D win2_mat = new PMatrix3D();
//    win2_mat.rotateZ(90 * toRadians);
    win2_mat.translate(0, 0, 125 * 2);
    window2.initBody(window2_dim, win2_mat, window2_rgb);
    createFitting(win2_mat, window2_dim);
    
    PMatrix3D win3_mat = new PMatrix3D();
    win3_mat.translate(0, 0, 125 * 3);
    window3.initBody(window3_dim, win3_mat, window3_rgb);
    createFitting(win3_mat, window3_dim);
    
//    PMatrix3D win4_mat = new PMatrix3D();
//    win4_mat.translate(0, 0, 125 * 4 + 50);
//    win4_mat.rotateY(90 * toRadians);
//    window4.initBody(window4_dim, win4_mat, window4_rgb);
//    createFitting(win4_mat, window4_dim, true);
  }
 

public void addCube(float cx, float cy, float cz, float dim, int depth, int max_depth){
  float mass = dim*dim*dim;
  BObject body = new BBox(this, mass, dim, dim, dim);

  // setup initial body transform-matrix
  PMatrix3D mat_p5 = new PMatrix3D();
  mat_p5.translate(cx, cy, cz);

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

  Transform transform = new Transform(mat);
  
  // rigid-body properties
  body.rigidBody.setWorldTransform(transform);
  body.rigidBody.setRestitution(.1f);
  body.rigidBody.setFriction(0.85f);
  body.rigidBody.setDamping(0.2f, 0.2f);
  
  
  int[] rgb = menger_color[depth];
  float r = rgb[0];
  float g = rgb[1];
  float b = rgb[2];
  
  
  body.displayShape = createShape(BOX, dim, dim, dim);
  body.displayShape.setStroke(false);
  body.displayShape.setFill(true);
  body.displayShape.setStroke(color(0));
  body.displayShape.setFill(color(r,g,b));
  
  if(depth != max_depth){   
    body.displayShape.setName("cube [wire1]");
  } else {
    body.displayShape.setName("cube [wire2]" );
  }
  
  physics.addBody(body);
  group_bulletbodies.addChild(body.displayShape);
}