processing マルチマーカーを使用して巨大なARをする

import codeanticode.gsvideo.*;

import jp.nyatla.nyar4psg.*;

GSCapture cam;

MultiMarker nya;

int num=10;

PMatrix3D trans[]=new PMatrix3D[num];

boolean had_detect[]=new boolean[num];

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

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

nya.addNyIdMarker(i, 40);

}

cam.start();

trans[0]=new PMatrix3D(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);

had_detect[0]=true;

}

void draw()

{

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

return;

}

cam.read();

nya.detect(cam);

background(0);

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

ArrayList<Integer> detectmarker=new ArrayList<Integer>();//認識に成功したマーカーのidを登録

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

if(nya.isExistMarker(i)){

detectmarker.add(i);

println(i);

}

}

int detectnum=detectmarker.size();//認識できたマーカーの数

println("認識できたまーかー："+detectnum);

if(detectnum==0){

return;

}

if(detectnum==1){

int id=detectmarker.get(0);

if(had_detect[id]){

//マーカーが1つでそれが認識済みだった場合

nya.beginTransform(id);

PMatrix3D A=nya.getMarkerMatrix(id);

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

A.apply(B);

setMatrix(A);

drawobject();

nya.endTransform();

}

}else{

//認識できたマーカーが複数の場合

if(nya.isExistMarker(0)){

//基準マーカーが見えている

nya.beginTransform(0);

drawobject();

nya.endTransform();

//他のマーカーとの位置関係登録

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

int id=detectmarker.get(i);

if(id==0)continue;

matrixload(id);//位置関係の登録

had_detect[id]=true;

}

}else{

//基準マーカーがみえていない

int used_marker=-1;

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

int id=detectmarker.get(i);

if(had_detect[id]){

used_marker=id;

break;

}

}

if(used_marker==-1)return;//座標系の保管されているマーカーがないとき、描画しない

nya.beginTransform(used_marker);

PMatrix3D A=nya.getMarkerMatrix(used_marker);

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

A.apply(B);

setMatrix(A);

drawobject();

nya.endTransform();

//他のマーカーとの位置関係登録

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

int id=detectmarker.get(i);

if(id==used_marker)continue;

//matrixload(id,used_marker);//位置関係の登録

matrixload(id,A);//位置関係の登録

had_detect[id]=true;

}

}

}

}

void matrixload(int i) {

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

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

trans[i].invert();

PMatrix3D trans2=nya.getMarkerMatrix(0);

trans[i].apply(trans2);

}

void matrixload(int i,PMatrix3D A) {

//iのマーカーからjのマーカーに移動

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

trans[i].invert();

PMatrix3D trans2=new PMatrix3D(A);

trans[i].apply(trans2);

}

void drawobject(){

fill(0,255,0);

translate(0,0,100);

box(200);

println(had_detect[4]);

}