Processing NyAR4Psgでマルチマーカー

import codeanticode.gsvideo.*;

import jp.nyatla.nyar4psg.*;

GSCapture cam;

MultiMarker nya;

PMatrix3D trans[]=new PMatrix3D[2];

void setup() {

size(640, 480, P3D);

colorMode(RGB, 100);

println(MultiMarker.VERSION);

cam=new GSCapture(this, 640, 480);

nya=new MultiMarker(this, width, height, "camera_para.dat", NyAR4PsgConfig.CONFIG_PSG);

nya.addNyIdMarker(0, 40);

nya.addNyIdMarker(1, 40);

cam.start();

trans[0]=new PMatrix3D(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);//単位行列～

}

void draw()

{

if (cam.available() !=true) {

return;

}

cam.read();

nya.detect(cam);

background(0);

nya.drawBackground(cam);//frustumを考慮した背景描画

if (nya.isExistMarker(0)&&nya.isExistMarker(1)) {

nya.beginTransform(0);

fill(255, 0, 0);

translate(0, 0, 20);

box(40);

matrixload();

nya.endTransform();

}

else {

for (int i=0;i<2;i++) {

if (!nya.isExistMarker(i))continue;

nya.beginTransform(i);

PMatrix3D A=nya.getMarkerMatrix(i);

PMatrix3D B=new PMatrix3D(trans[i]);

A.apply(B);

setMatrix(A);

fill(255, 0, 0);

translate(0, 0, 20);

box(40);

nya.endTransform();

}

}

}

void matrixload() {

//1のマーカーから0のマーカーに移動

trans[1]=nya.getMarkerMatrix(1);

trans[1].invert();

PMatrix3D trans2=nya.getMarkerMatrix(0);

trans[1].apply(trans2);

}

PMatrix3D A=nya.getMarkerMatrix(i);

PMatrix3D B=new PMatrix3D(trans[i]);

A.apply(B);

setMatrix(A);