From b9d36ea177e7c55e12e12d3e32efba6cb31b89f9 Mon Sep 17 00:00:00 2001 From: nyatla Date: Sat, 7 Jun 2008 02:37:50 +0000 Subject: [PATCH] =?utf8?q?[=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B9]NyARToolki?= =?utf8?q?t=201.0.0?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit --- changelog/0.6.20080330.0.txt | 30 - changelog/0.7.20080406.0.txt | 595 ---- changelog/0.8.20080412.0.txt | 3158 -------------------- readme.ja.txt | 8 +- sample/java3d/.classpath | 6 +- .../java3d/utils/J3dNyARRaster_RGB.java | 50 +- .../utils/NyARSingleMarkerBehaviorHolder.java | 4 +- .../nyartoolkit/jmf/utils/JmfNyARRaster_RGB.java | 40 +- .../jogl/utils/GLNyARSingleDetectMarker.java | 2 +- src/jp/nyatla/nyartoolkit/base/Param.java | 6 +- src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java | 193 +- .../nyatla/nyartoolkit/core/NyARColorPatt_O1.java | 268 ++ .../nyatla/nyartoolkit/core/NyARColorPatt_O2.java | 281 ++ .../nyatla/nyartoolkit/core/NyARColorPatt_O3.java | 340 +++ .../nyatla/nyartoolkit/core/NyARDetectMarker.java | 566 ++-- .../nyatla/nyartoolkit/core/NyARDetectSquare.java | 153 +- src/jp/nyatla/nyartoolkit/core/NyARLabeling.java | 600 ++-- src/jp/nyatla/nyartoolkit/core/NyARMarker.java | 229 +- src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java | 148 + src/jp/nyatla/nyartoolkit/core/NyARMat.java | 754 +++-- src/jp/nyatla/nyartoolkit/core/NyARParam.java | 211 +- src/jp/nyatla/nyartoolkit/core/NyARSquare.java | 95 +- src/jp/nyatla/nyartoolkit/core/NyARSquareList.java | 70 + src/jp/nyatla/nyartoolkit/core/NyARTransMat.java | 989 ++---- src/jp/nyatla/nyartoolkit/core/NyARTransRot.java | 1198 ++++++++ src/jp/nyatla/nyartoolkit/core/NyARVec.java | 265 +- .../core/match/NyARMatchPatt_BlackWhite.java | 2 +- .../match/NyARMatchPatt_Color_WITHOUT_PCA.java | 129 +- .../core/match/NyARMatchPatt_Color_WITH_PCA.java | 2 +- .../nyatla/nyartoolkit/core/raster/NyARRaster.java | 21 +- .../nyartoolkit/core/raster/NyARRaster_BGRA.java | 37 +- .../nyartoolkit/core/raster/NyARRaster_Blank.java | 16 +- .../nyartoolkit/core/raster/NyARRaster_RGB.java | 37 +- .../detector/NyARSingleDetectMarker.java | 56 +- src/jp/nyatla/nyartoolkit/sample/RawFileTest.java | 13 +- src/jp/nyatla/util/IntValue.java | 26 +- 36 files changed, 4578 insertions(+), 6020 deletions(-) delete mode 100644 changelog/0.6.20080330.0.txt delete mode 100644 changelog/0.7.20080406.0.txt delete mode 100644 changelog/0.8.20080412.0.txt create mode 100644 src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O1.java create mode 100644 src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java create mode 100644 src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O3.java create mode 100644 src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java create mode 100644 src/jp/nyatla/nyartoolkit/core/NyARSquareList.java create mode 100644 src/jp/nyatla/nyartoolkit/core/NyARTransRot.java diff --git a/changelog/0.6.20080330.0.txt b/changelog/0.6.20080330.0.txt deleted file mode 100644 index 0dba4ee..0000000 --- a/changelog/0.6.20080330.0.txt +++ /dev/null @@ -1,30 +0,0 @@ -2008.03.30 version 0.6.20080330.0 - -*Title --メモリ管理の見直し --速度向上(84%up) --サンプルの15fps化 - -*PublicAPI - --NyARCode.java -[改名--rename] - NyARCode::LoadFromARFileをNyARCode::loadFromARFile - --NyARDetectMarker.java -[廃止--delete] -NyARDetectMarker::getMarkerArray -[追加--add] -NyARDetectMarker::getMarker - - --NyARMat.java -[更新--update] -NyARMat::matrixSelfInv -NyARMat::matrixDup - --DoubleValue.java -[廃止--delete] -DoubleValue::set -DoubleValue::get -DoubleValue::add diff --git a/changelog/0.7.20080406.0.txt b/changelog/0.7.20080406.0.txt deleted file mode 100644 index 705f134..0000000 --- a/changelog/0.7.20080406.0.txt +++ /dev/null @@ -1,595 +0,0 @@ -2008.04.06 version 0.7.20080406.0 - -*Title --バグフィクス ---キャプチャ画像のBGRをRGBとして評価していたのを修正 ---JMFキャプチャ画像ピクセルフォーマット認識の自動化 ---サンプルバグ修正 ---スペルミス修正 - - -Index: D:/project.files/java.Lab/NyARToolKit/sample/jogl/JavaSimpleLite.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/jogl/JavaSimpleLite.java (revision 172) -+++ D:/project.files/java.Lab/NyARToolKit/sample/jogl/JavaSimpleLite.java (revision 173) -@@ -121,9 +121,8 @@ - //NyARToolkitの準備 - try{ - //キャプチャの準備 -- capture=new JmfCameraCapture(320,240,15f,JmfCameraCapture.PIXCEL_FORMAT_RGB); -+ capture=new JmfCameraCapture(320,240,15f,JmfCameraCapture.PIXEL_FORMAT_RGB); - capture.setCaptureListener(this); -- capture.start(); - //NyARToolkitの準備 - ar_param=new GLNyARParam(); - NyARCode ar_code =new NyARCode(16,16); -@@ -132,8 +131,9 @@ - nya=new GLNyARSingleDetectMarker(ar_param,ar_code,80.0); - ar_code.loadFromARFile(CARCODE_FILE); - //GL対応のRGBラスタオブジェクト -- cap_image=new GLNyARRaster_RGB(gl,ar_param,320,240); -- -+ cap_image=new GLNyARRaster_RGB(gl,ar_param); -+ //キャプチャ開始 -+ capture.start(); - }catch(Exception e){ - e.printStackTrace(); - } -@@ -166,17 +166,10 @@ - { - - try{ -- gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT); // Clear the buffers for new frame. -- -- -- //キャプチャしたイメージを加工 -- Buffer b=capture.readBuffer(); -- //BufferToImage b2i=new BufferToImage((VideoFormat)b.getFormat()); -- if(b.getData()==null){ -+ if(!cap_image.hasData()){ - return; -- }else{ -- //画像準備OK -- } -+ } -+ gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT); // Clear the buffers for new frame. - //画像チェックしてマーカー探して、背景を書く - boolean is_marker_exist; - synchronized(cap_image){ -@@ -206,7 +199,7 @@ - { - try{ - synchronized(cap_image){ -- cap_image.setRawData((byte[])i_buffer.getData(), true); -+ cap_image.setBuffer(i_buffer, true); - } - }catch(Exception e){ - e.printStackTrace(); -Index: D:/project.files/java.Lab/NyARToolKit/sample/jogl/jp/nyatla/nyartoolkit/gutil/GLNyARRaster_RGB.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/jogl/jp/nyatla/nyartoolkit/gutil/GLNyARRaster_RGB.java (revision 172) -+++ D:/project.files/java.Lab/NyARToolKit/sample/jogl/jp/nyatla/nyartoolkit/gutil/GLNyARRaster_RGB.java (revision 173) -@@ -1,5 +1,6 @@ - /** -- * NyARRaster_RGBにOpenGL向け関数を追加したもの -+ * NyARRaster_RGBにOpenGL向け関数を追加したものです。 -+ * - * (c)2008 R.iizuka - * airmail(at)ebony.plala.or.jp - * http://nyatla.jp/ -@@ -12,37 +13,58 @@ - import javax.media.opengl.GL; - import javax.media.opengl.glu.GLU; - -+import jp.nyatla.nyartoolkit.NyARException; - import jp.nyatla.nyartoolkit.core.NyARParam; --import jp.nyatla.nyartoolkit.core.raster.NyARRaster_RGB; -+import jp.nyatla.nyartoolkit.jmfutil.*; - --public class GLNyARRaster_RGB extends NyARRaster_RGB -+public class GLNyARRaster_RGB extends JmfNyARRaster_RGB - { - private NyARParam cparam; - private GL ref_gl; - private GLU glu; -- public GLNyARRaster_RGB(GL i_ref_gl,NyARParam i_cparam,int i_width,int i_height) -+ private byte[] gl_buf; -+ private int gl_flag; -+ -+ public GLNyARRaster_RGB(GL i_ref_gl,NyARParam i_cparam) - { -- width=i_width; -- height=i_height; -+ super(i_cparam.getX(),i_cparam.getY()); -+ gl_flag=GL.GL_RGB; - cparam=i_cparam; - ref_gl=i_ref_gl; - glu=new GLU(); -- this.ref_buf=new byte[i_width*i_height*3]; -+ this.gl_buf=new byte[width*height*3]; - } -- public void setRawData(byte[] i_buf,boolean i_is_reverse) -+ public void setBuffer(javax.media.Buffer i_buffer,boolean i_is_reverse) throws NyARException - { -+ super.setBuffer(i_buffer); -+ //メモ：この時点では、ref_dataにはi_bufferの参照値が入ってる。 -+ -+ //GL用のデータを準備 - if(i_is_reverse){ - int length=width*3; - int src_idx=0; - int dest_idx=(height-1)*length; - for(int i=0;i0){//if( *pnt1 > 0 ) { - label_img[p2][i]=label_img[p1][i];//*pnt2 = *pnt1; -Index: D:/project.files/java.Lab/NyARToolKit/src/jp/nyatla/nyartoolkit/core/NyARSquare.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/src/jp/nyatla/nyartoolkit/core/NyARSquare.java (revision 172) -+++ D:/project.files/java.Lab/NyARToolKit/src/jp/nyatla/nyartoolkit/core/NyARSquare.java (revision 173) -@@ -36,19 +36,19 @@ - * - */ - public class NyARSquare{ -- private NyARMarker marker; -- public int area; -- public double[] pos; -- public double[][] line; //double[4][3] -- public double[][] vertex;//double[4][2]; -+ private NyARMarker marker; -+ public int area; -+ public double[] pos; -+ public double[][] line; //double[4][3] -+ public double[][] vertex;//double[4][2]; - public NyARSquare(NyARMarker i_marker,double[][] i_attached_line,double[][] i_attached_vertex) - { -- //ARSquareは、ARMarkerを完全にラップするようにした。 -- marker=i_marker; -- area=i_marker.area; -- pos =i_marker.pos; -- line =i_attached_line; -- vertex=i_attached_vertex; -+ //ARSquareは、ARMarkerを完全にラップするようにした。 -+ marker=i_marker; -+ area=i_marker.area; -+ pos =i_marker.pos; -+ line =i_attached_line; -+ vertex=i_attached_vertex; - } - public NyARMarker getMarker() - { diff --git a/changelog/0.8.20080412.0.txt b/changelog/0.8.20080412.0.txt deleted file mode 100644 index 2bccbce..0000000 --- a/changelog/0.8.20080412.0.txt +++ /dev/null @@ -1,3158 +0,0 @@ -Index: D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorListener.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorListener.java (revision 0) -+++ D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorListener.java (revision 178) -@@ -0,0 +1,24 @@ -+/** -+ * NyARToolkit‚ÌBehavior‚̃ŠƒXƒi -+ * (c)2008 AŒÕ—nyatla.jp -+ * airmail@ebony.plala.or.jp -+ * http://nyatla.jp/ -+ */ -+package jp.nyatla.nyartoolkit.java3d.utils; -+ -+import javax.media.j3d.*; -+ -+public interface NyARSingleMarkerBehaviorListener -+{ -+ /** -+ * ‚±‚̃ŠƒXƒi‚́AƒŠƒXƒi‚Ƀ}[ƒJ[‚ɘA“®‚µ‚ăIƒuƒWƒFƒNƒg‚𑀍삷‚éƒ`ƒƒƒ“ƒX‚ð—^‚¦‚Ü‚·B -+ * ƒŠƒXƒi‚ÍNyARSingleMarkerBehaviorŠÖ”“à‚ÌprocessStimulusŠÖ”‚©‚çŒÄ‚яo‚³‚ê‚Ü‚·B -+ * -+ * @param i_is_marker_exist -+ * ƒ}[ƒJ[‚ª‘¶Ý‚·‚éê‡trueA‘¶Ý‚µ‚È‚¢ê‡Afalse‚Å‚·B -+ * @param i_transform3d -+ * ƒ}[ƒJ[‚ª‘¶Ý‚·‚éê‡A‚»‚Ì•ÏŠ·s—ñ‚ªŽw’肳‚ê‚Ü‚·B -+ * i_is_marker_exist‚ªtrue‚ÌŽž‚¾‚¯—LŒø‚Å‚·B -+ */ -+ public void onUpdate(boolean i_is_marker_exist,Transform3D i_transform3d); -+} -Index: D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARRaster_RGB.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARRaster_RGB.java (revision 0) -+++ D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARRaster_RGB.java (revision 178) -@@ -0,0 +1,64 @@ -+/** -+ * NyARRaster_RGB‚ÉOpenGLŒü‚¯ŠÖ”‚ð’ljÁ‚µ‚½‚à‚Ì -+ * (c)2008 AŒÕ—nyatla.jp -+ * airmail(at)ebony.plala.or.jp -+ * http://nyatla.jp/ -+ */ -+package jp.nyatla.nyartoolkit.java3d.utils; -+ -+import java.awt.image.*; -+import java.awt.color.*; -+ -+import javax.media.j3d.ImageComponent; -+import javax.media.j3d.ImageComponent2D; -+ -+ -+ -+import jp.nyatla.nyartoolkit.NyARException; -+import jp.nyatla.nyartoolkit.core.NyARParam; -+import jp.nyatla.nyartoolkit.jmf.utils.*; -+ -+ -+public class J3dNyARRaster_RGB extends JmfNyARRaster_RGB -+{ -+// public ImageUpdater updater; -+ private ImageComponent2D imc2d; -+ private byte[] i2d_buf; -+ -+ public void setBuffer(javax.media.Buffer i_buffer) throws NyARException -+ { -+ super.setBuffer(i_buffer); -+ //ƒƒ‚F‚±‚ÌŽž“_‚ł́Aref_data‚É‚Íi_buffer‚ÌŽQÆ’l‚ª“ü‚Á‚Ä‚éB -+ synchronized(imc2d){ -+ //ƒLƒƒƒvƒ`ƒƒƒf[ƒ^‚ði2d‚̃oƒbƒtƒ@‚ɃRƒs[‚·‚éBi‚±‚êÈ—ª‚µ‚½‚¢‚È‚ cBj -+ System.arraycopy(ref_buf,0,i2d_buf,0,this.i2d_buf.length); -+ } -+ //‚±‚±‚Åref_buf‚ÌŽQÆ’l‚ðref_buf‚ÖˆÚ“® -+ ref_buf=i2d_buf; -+ } -+ public J3dNyARRaster_RGB(NyARParam i_cparam) -+ { -+ super(i_cparam.getX(),i_cparam.getY()); -+ -+ //RGB‚̃‰ƒXƒ^‚ðì‚éB -+// ColorSpace cs=ColorSpace.getInstance(ColorSpace.CS_LINEAR_RGB); -+// ComponentColorModel cm; -+// cm=new ComponentColorModel(cs,new int[]{8,8,8},false,false,ComponentColorModel.OPAQUE,DataBuffer.TYPE_BYTE); -+// java.awt.image.WritableRaster raster=cm.createCompatibleWritableRaster(width,height); -+// i2d_buf=((DataBufferByte)raster.getDataBuffer()).getData(); -+// BufferedImage background_image = new BufferedImage(cm,raster, false, null); -+ BufferedImage background_image = new BufferedImage(width,height,BufferedImage.TYPE_3BYTE_BGR); -+ i2d_buf=((DataBufferByte)background_image.getRaster().getDataBuffer()).getData(); -+ // -+ imc2d= new ImageComponent2D(ImageComponent2D.FORMAT_RGB, background_image, true, true); -+ imc2d.setCapability(ImageComponent.ALLOW_IMAGE_WRITE); -+ } -+ /** -+ * ‚±‚̃IƒuƒWƒFƒNƒg‚ƘA“®‚·‚éImageComponent2DƒIƒuƒWƒFƒNƒg‚ÌŽQÆ’l‚𓾂éB -+ * @return -+ */ -+ public ImageComponent2D getImageComponent2D() -+ { -+ return imc2d; -+ } -+} -Index: D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARParam.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARParam.java (revision 0) -+++ D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARParam.java (revision 178) -@@ -0,0 +1,119 @@ -+/** -+ * NyARParam‚ÉOpenGLŒü‚¯ŠÖ”‚ð’ljÁ‚µ‚½‚à‚Ì -+ * (c)2008 AŒÕ—nyatla.jp -+ * airmail(at)ebony.plala.or.jp -+ * http://nyatla.jp/ -+ */ -+package jp.nyatla.nyartoolkit.java3d.utils; -+ -+import jp.nyatla.nyartoolkit.core.*; -+import javax.media.j3d.Transform3D; -+ -+public class J3dNyARParam extends NyARParam -+{ -+ private double view_distance_min=0.01;//1cm`10.0m -+ private double view_distance_max=10.0; -+ private Transform3D m_projection=null; -+ /** -+ * Ž‹‘̐ς̋߂¢•û‚ðƒ[ƒgƒ‹‚ÅŽw’è -+ * @param i_new_value -+ */ -+ public void setViewDistanceMin(double i_new_value) -+ { -+ m_projection=null;//ƒLƒƒƒbƒVƒ…Ï•Ï”‰Šú‰» -+ view_distance_min=i_new_value; -+ } -+ /** -+ * Ž‹‘̐ς̉“‚¢•û‚ðƒ[ƒgƒ‹‚ÅŽw’è -+ * @param i_new_value -+ */ -+ public void setViewDistanceMax(double i_new_value) -+ { -+ m_projection=null;//ƒLƒƒƒbƒVƒ…Ï•Ï”‰Šú‰» -+ view_distance_max=i_new_value; -+ } -+ /** -+ * void arglCameraFrustumRH(const ARParam *cparam, const double focalmin, const double focalmax, GLdouble m_projection[16]) -+ * ŠÖ”‚Ì’u‚«Š·‚¦ -+ * @param focalmin -+ * @param focalmax -+ * @return -+ */ -+ public Transform3D getCameraTransform() -+ { -+ //Šù‚É’l‚ªƒLƒƒƒbƒVƒ…‚³‚ê‚Ä‚¢‚½‚ç‚»‚ê‚ðŽg‚¤ -+ if(m_projection!=null){ -+ return m_projection; -+ } -+ //–³‚¯‚ê‚ÎŒvŽZ -+ -+ NyARMat trans_mat=new NyARMat(3,4); -+ NyARMat icpara_mat=new NyARMat(3,4); -+ double[][] p=new double[3][3], q=new double[4][4]; -+ double width, height; -+ int i, j; -+ -+ width = xsize; -+ height = ysize; -+ -+ decompMat(icpara_mat,trans_mat); -+ -+ double[][] icpara=icpara_mat.getArray(); -+ double[][] trans=trans_mat.getArray(); -+ for (i = 0; i < 4; i++) { -+ icpara[1][i] = (height - 1)*(icpara[2][i]) - icpara[1][i]; -+ } -+ -+ for(i = 0; i < 3; i++) { -+ for(j = 0; j < 3; j++) { -+ p[i][j] = icpara[i][j] / icpara[2][2]; -+ } -+ } -+ //p[0][0],p[1][1]=n -+ //p[0][2],p[1][2]=t+b -+ -+ //Projection‚ÌŒvŽZ -+ q[0][0] = (2.0 * p[0][0] / (width - 1)); -+ q[0][1] = (2.0 * p[0][1] / (width - 1)); -+ q[0][2] = -((2.0 * p[0][2] / (width - 1)) - 1.0); -+ q[0][3] = 0.0; -+ -+ q[1][0] = 0.0; -+ q[1][1] = -(2.0 * p[1][1] / (height - 1)); -+ q[1][2] = -((2.0 * p[1][2] / (height - 1)) - 1.0); -+ q[1][3] = 0.0; -+ -+ q[2][0] = 0.0; -+ q[2][1] = 0.0; -+ q[2][2] = (view_distance_max + view_distance_min)/(view_distance_min - view_distance_max); -+ q[2][3] = 2.0 * view_distance_max * view_distance_min / (view_distance_min - view_distance_max); -+ -+ q[3][0] = 0.0; -+ q[3][1] = 0.0; -+ q[3][2] = -1.0; -+ q[3][3] = 0.0; -+ -+ q[2][2]=q[2][2]*-1; -+ q[2][3]=q[2][3]*-1; -+ -+ double[] tmp_projection=new double[16]; -+ for (i = 0; i < 4; i++) { // Row. -+ // First 3 columns of the current row. -+ for (j = 0; j < 3; j++) { // Column. -+ tmp_projection[i + j*4] =( -+ q[i][0] * trans[0][j] + -+ q[i][1] * trans[1][j] + -+ q[i][2] * trans[2][j]); -+ } -+ // Fourth column of the current row. -+ tmp_projection[i + 3*4]= -+ q[i][0] * trans[0][3] + -+ q[i][1] * trans[1][3] + -+ q[i][2] * trans[2][3] + -+ q[i][3]; -+ } -+ m_projection=new Transform3D(tmp_projection); -+ m_projection.transpose(); -+ return m_projection; -+ } -+} -Index: D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorHolder.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorHolder.java (revision 0) -+++ D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorHolder.java (revision 178) -@@ -0,0 +1,191 @@ -+/** -+ * NyARToolkit‚ÌBehaviorƒzƒ‹ƒ_[ -+ * (c)2008 AŒÕ—nyatla.jp -+ * airmail@ebony.plala.or.jp -+ * http://nyatla.jp/ -+ */ -+package jp.nyatla.nyartoolkit.java3d.utils; -+ -+import java.util.Enumeration; -+ -+import javax.media.Buffer; -+import javax.media.j3d.*; -+import javax.vecmath.*; -+ -+import jp.nyatla.nyartoolkit.core.NyARParam; -+import jp.nyatla.nyartoolkit.detector.NyARSingleDetectMarker; -+import jp.nyatla.nyartoolkit.jmf.*; -+import jp.nyatla.nyartoolkit.jmf.utils.JmfCameraCapture; -+import jp.nyatla.nyartoolkit.jmf.utils.JmfCaptureListener; -+import jp.nyatla.nyartoolkit.*; -+import jp.nyatla.nyartoolkit.core.*; -+ -+/** -+ * NyARToolkit‚ƘA“®‚µ‚½Behavior‚ð•Ô‹p‚·‚éƒNƒ‰ƒX‚Å‚·B -+ * ’ñ‹Ÿ‚Å‚«‚éBehavior‚́ABackground‚ÆTransformgroup‚Å‚·B -+ * -+ */ -+public class NyARSingleMarkerBehaviorHolder implements JmfCaptureListener -+{ -+ private NyARParam cparam; -+ private JmfCameraCapture capture; -+ private J3dNyARRaster_RGB nya_raster;//Å‘å3ƒXƒŒƒbƒh‚Å‹¤—L‚³‚ê‚é‚̂ŁA”r‘¼§Œä‚©‚¯‚邱‚ƁB -+ private NyARSingleDetectMarker nya; -+ //Behaviorƒzƒ‹ƒ_ -+ private NyARBehavior nya_behavior; -+ public NyARSingleMarkerBehaviorHolder(NyARParam i_cparam,float i_rate,NyARCode i_ar_code,double i_marker_width) throws NyARException -+ { -+ nya_behavior=null; -+ cparam=i_cparam; -+ capture=new JmfCameraCapture(cparam.getX(),cparam.getY(),i_rate,JmfCameraCapture.PIXEL_FORMAT_RGB); -+ capture.setCaptureListener(this); -+ nya_raster=new J3dNyARRaster_RGB(cparam); -+ nya=new NyARSingleDetectMarker(cparam,i_ar_code,i_marker_width); -+ nya_behavior=new NyARBehavior(nya,nya_raster,i_rate); -+ } -+ public Behavior getBehavior() -+ { -+ return nya_behavior; -+ } -+ /** -+ * i_back_ground‚ɃLƒƒƒvƒ`ƒƒ‰æ‘œ‚ð“]‘—‚·‚é‚悤‚ÉBehavior‚ðÝ’肵‚Ü‚·B -+ * i_back_groung‚ÍALLOW_IMAGE_WRITE‘®«‚ðŽ‚Â‚à‚Ì‚Å‚ ‚é•K—v‚ª‚ ‚è‚Ü‚·B -+ * @param i_back_groung -+ * @return -+ */ -+ public void setBackGround(Background i_back_ground) -+ { -+ //ƒR[ƒ‹æ‚Å”r‘¼§Œä -+ nya_behavior.setRelatedBackGround(i_back_ground); -+ } -+ /** -+ * i_trgroup‚̍À•WŒn‚ðƒ}[ƒJ[‚É‚ ‚킹‚é‚悤‚ÉBehavior‚ðÝ’肵‚Ü‚·B -+ * -+ */ -+ public void setTransformGroup(TransformGroup i_trgroup) -+ { -+ //ƒR[ƒ‹æ‚Å”r‘¼§Œä -+ nya_behavior.setRelatedTransformGroup(i_trgroup); -+ } -+ /** -+ * À•WŒnÄŒvŽZŒã‚ɌĂяo‚³‚ê‚郊ƒXƒi‚Å‚·B -+ * @param i_listener -+ */ -+ public void setUpdateListener(NyARSingleMarkerBehaviorListener i_listener) -+ { -+ //ƒR[ƒ‹æ‚Å”r‘¼§Œä -+ nya_behavior.setUpdateListener(i_listener); -+ } -+ /** -+ * ƒ‰ƒXƒ^‚ðXV ƒR[ƒ‹ƒoƒbƒNŠÖ”‚¾‚©‚çŒÄ‚ñ‚¶‚á‚ç‚ß‚¦ -+ */ -+ public void onUpdateBuffer(Buffer i_buffer) -+ { -+ try{ -+ synchronized(nya_raster){ -+ nya_raster.setBuffer(i_buffer); -+ } -+ }catch(Exception e){ -+ e.printStackTrace(); -+ } -+ } -+ public void start() throws NyARException -+ { -+ //ŠJŽn -+ capture.start(); -+ } -+ public void stop() -+ { -+ capture.stop(); -+ } -+} -+ -+ -+class NyARBehavior extends Behavior -+{ -+ private NyARSingleDetectMarker related_nya; -+ private TransformGroup trgroup; -+ private Background back_ground; -+ private J3dNyARRaster_RGB raster; -+ private WakeupCondition wakeup; -+ private NyARSingleMarkerBehaviorListener listener; -+ public void initialize() -+ { -+ wakeupOn(wakeup); -+ } -+ /** -+ * i_related_ic2d‚Ì“à—e‚Å’èŠú“I‚Éi_back_ground‚ðXV‚·‚éBehavior -+ * @param i_back_ground -+ * @param i_related_ic2d -+ */ -+ public NyARBehavior(NyARSingleDetectMarker i_related_nya,J3dNyARRaster_RGB i_related_raster,float i_rate) -+ { -+ super(); -+ wakeup=new WakeupOnElapsedTime((int)(1000/i_rate)); -+ related_nya=i_related_nya; -+ trgroup =null; -+ raster =i_related_raster; -+ back_ground=null; -+ listener=null; -+ this.setSchedulingBounds(new BoundingSphere(new Point3d(), 100.0)); -+ } -+ public void setRelatedBackGround(Background i_back_ground) -+ { -+ synchronized(raster){ -+ back_ground=i_back_ground; -+ } -+ } -+ public void setRelatedTransformGroup(TransformGroup i_trgroup) -+ { -+ synchronized(raster){ -+ trgroup=i_trgroup; -+ } -+ } -+ public void setUpdateListener(NyARSingleMarkerBehaviorListener i_listener) -+ { -+ synchronized(raster){ -+ listener=i_listener; -+ } -+ } -+ -+ /** -+ * ‚¢‚í‚ä‚éƒCƒxƒ“ƒgƒnƒ“ƒhƒ‰ -+ */ -+ public void processStimulus(Enumeration criteria) -+ { -+ try{ -+ synchronized(raster){ -+ Transform3D t3d=null; -+ boolean is_marker_exist=false; -+ if(back_ground!=null){ -+ back_ground.setImage(raster.getImageComponent2D()); -+ } -+ if(raster.hasData()){ -+ is_marker_exist=related_nya.detectMarkerLite(raster, 100); -+ if(is_marker_exist){ -+ NyARMat nymat=related_nya.getTransmationMatrix(); -+ double[][] src=nymat.getArray(); -+ Matrix4d matrix=new Matrix4d( -+ src[0][0],-src[1][0],-src[2][0],0, -+ -src[0][1], src[1][1], src[2][1],0, -+ -src[0][2], src[1][2], src[2][2],0, -+ -src[0][3], src[1][3], src[2][3],1); -+ matrix.transpose(); -+ t3d=new Transform3D(matrix); -+ if(trgroup!=null){ -+ trgroup.setTransform(t3d); -+ } -+ } -+ } -+ if(listener!=null) -+ { -+ listener.onUpdate(is_marker_exist, t3d); -+ } -+ } -+ wakeupOn(wakeup); -+ }catch(Exception e){ -+ e.printStackTrace(); -+ } -+ } -+} -+ -Index: D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/sample/NyARJava3D.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/sample/NyARJava3D.java (revision 0) -+++ D:/project.files/java.Lab/NyARToolKit/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/sample/NyARJava3D.java (revision 178) -@@ -0,0 +1,148 @@ -+/** -+ * Java3DƒTƒ“ƒvƒ‹ƒvƒƒOƒ‰ƒ€ -+ * ƒVƒ“ƒOƒ‹ƒ}[ƒJ[’ǐ՗p‚ÌBehavior‚ðŽg‚Á‚āA”wŒi‚Æ‚PŒÂ‚̃}[ƒJ[‚ɘA“®‚µ‚½TransformGroup -+ * ‚ð“®‚©‚µ‚Ü‚·B -+ * (c)2008 AŒÕ—nyatla.jp -+ * airmail@ebony.plala.or.jp -+ * http://nyatla.jp/ -+ */ -+package jp.nyatla.nyartoolkit.java3d.sample; -+ -+import java.awt.BorderLayout; -+import javax.media.j3d.*; -+ -+import com.sun.j3d.utils.universe.*; -+import java.awt.*; -+import javax.swing.JFrame; -+import javax.vecmath.*; -+ -+import jp.nyatla.nyartoolkit.core.*; -+import jp.nyatla.nyartoolkit.java3d.utils.*; -+ -+import com.sun.j3d.utils.geometry.ColorCube; -+ -+public class NyARJava3D extends JFrame implements NyARSingleMarkerBehaviorListener -+{ -+ private final String CARCODE_FILE ="../../Data/patt.hiro"; -+ private final String PARAM_FILE ="../../Data/camera_para.dat"; -+ //NyARToolkitŠÖŒW -+ private NyARSingleMarkerBehaviorHolder nya_behavior; -+ private J3dNyARParam ar_param; -+ //universeŠÖŒW -+ private Canvas3D canvas; -+ private Locale locale; -+ private VirtualUniverse universe; -+ public static void main(String[] args) -+ { -+ try{ -+ NyARJava3D frame = new NyARJava3D(); -+ -+ frame.setVisible(true); -+ Insets ins=frame.getInsets(); -+ frame.setSize(320+ins.left+ins.right,240+ins.top+ins.bottom); -+ frame.startCapture(); -+ frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); -+ }catch(Exception e){ -+ e.printStackTrace(); -+ } -+ } -+ public void onUpdate(boolean i_is_marker_exist,Transform3D i_transform3d) -+ { -+ /* -+ * TODO:Please write your behavior operation code here. -+ * ƒ}[ƒJ[‚ÌŽp¨‚ðŒ³‚É‘¼‚Ì‚RDƒIƒuƒWƒFƒNƒg‚𑀍삷‚é‚Æ‚«‚́A‚±‚±‚ɏˆ—‚ð‘‚«‚Ü‚·B*/ -+ -+ } -+ public void startCapture() throws Exception -+ { -+ nya_behavior.start(); -+ } -+ public NyARJava3D() throws Exception -+ { -+ super("Java3D Example NyARToolkit"); -+ -+ //NyARToolkit‚̏€”õ -+ NyARCode ar_code =new NyARCode(16,16); -+ ar_code.loadFromARFile(CARCODE_FILE); -+ ar_param=new J3dNyARParam(); -+ ar_param.loadFromARFile(PARAM_FILE); -+ ar_param.changeSize(320,240); -+ -+ //locale‚̍쐬‚Ælocate‚Æview‚̐ݒè -+ universe = new VirtualUniverse(); -+ locale = new Locale( universe ); -+ canvas=new Canvas3D( SimpleUniverse.getPreferredConfiguration()); -+ View view = new View(); -+ ViewPlatform viewPlatform = new ViewPlatform(); -+ view.attachViewPlatform( viewPlatform ); -+ view.addCanvas3D(canvas); -+ view.setPhysicalBody( new PhysicalBody() ); -+ view.setPhysicalEnvironment( new PhysicalEnvironment()); -+ -+ //Ž‹ŠE‚̐ݒè(ƒJƒƒ‰Ý’è‚©‚çŽæ“¾) -+ Transform3D camera_3d=ar_param.getCameraTransform(); -+ view.setCompatibilityModeEnable(true); -+ view.setProjectionPolicy(View.PERSPECTIVE_PROJECTION); -+ view.setLeftProjection(camera_3d); -+ -+ //Ž‹“_Ý’è(0,0,0‚©‚çAYŽ²‚ð180“x‰ñ“]‚µ‚ÄZ+•ûŒü‚ðŒü‚­‚悤‚É‚·‚éB) -+ TransformGroup viewGroup = new TransformGroup(); -+ Transform3D viewTransform = new Transform3D(); -+ viewTransform.rotY(Math.PI); -+ viewTransform.setTranslation(new Vector3d(0.0,0.0,0.0)); -+ viewGroup.setTransform( viewTransform ); -+ viewGroup.addChild( viewPlatform ); -+ BranchGroup viewRoot = new BranchGroup(); -+ viewRoot.addChild( viewGroup ); -+ locale.addBranchGraph( viewRoot ); -+ -+ -+ //ƒoƒbƒNƒOƒ‰ƒEƒ“ƒh‚̍쐬 -+ Background background =new Background(); -+ BoundingSphere bounds = new BoundingSphere(); -+ bounds.setRadius( 10.0 ); -+ background.setApplicationBounds(bounds); -+ background.setImageScaleMode(Background.SCALE_FIT_ALL); -+ background.setCapability(Background.ALLOW_IMAGE_WRITE); -+ BranchGroup root = new BranchGroup(); -+ root.addChild(background); -+ -+ //TransformGroup‚ň͂Á‚½ƒV[ƒ“ƒOƒ‰ƒt‚̍쐬 -+ TransformGroup transform=new TransformGroup(); -+ transform.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE); -+ transform.addChild(createSceneGraph()); -+ root.addChild(transform); -+ -+ //NyARToolkit‚ÌBehavior‚ðì‚éB(ƒ}[ƒJ[ƒTƒCƒY‚̓[ƒgƒ‹‚ÅŽw’è‚·‚邱‚Æ) -+ nya_behavior=new NyARSingleMarkerBehaviorHolder(ar_param,30f,ar_code,0.08); -+ //Behavior‚ɘA“®‚·‚éƒOƒ‹[ƒv‚ðƒZƒbƒg -+ nya_behavior.setTransformGroup(transform); -+ nya_behavior.setBackGround(background); -+ -+ //o—ˆ‚½behavior‚ðƒZƒbƒg -+ root.addChild(nya_behavior.getBehavior()); -+ nya_behavior.setUpdateListener(this); -+ -+ //•\Ž¦ƒuƒ‰ƒ“ƒ`‚ðLocate‚ɃZƒbƒg -+ locale.addBranchGraph(root); -+ -+ //ƒEƒCƒ“ƒhƒE‚̐ݒè -+ setLayout(new BorderLayout()); -+ add(canvas,BorderLayout.CENTER); -+ } -+ /** -+ * ƒV[ƒ“ƒOƒ‰ƒt‚ðì‚Á‚āA‚»‚̃m[ƒh‚ð•Ô‚·B -+ * ‚±‚̃m[ƒh‚Í40mm‚̐F‚‚«ƒiƒ^ƒfƒRƒR‚ð•\Ž¦‚·‚éƒV[ƒ“B‚šŽ²‚ðŠî€‚É20mmã‚É•‚‚©‚¹‚Ä‚éB -+ * @return -+ */ -+ private Node createSceneGraph() -+ { -+ TransformGroup tg=new TransformGroup(); -+ Transform3D mt=new Transform3D(); -+ mt.setTranslation(new Vector3d(0.00,0.0,20*0.001)); -+ // ‘å‚«‚³ 40mm‚̐F•t‚«—§•û‘Ì‚ðAZŽ²ã‚Å20mm“®‚©‚µ‚Ä”z’uj -+ tg.setTransform(mt); -+ tg.addChild(new ColorCube(20*0.001)); -+ return tg; -+ } -+} -Index: D:/project.files/java.Lab/NyARToolKit/sample/jogl/JavaSimpleLite.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/jogl/JavaSimpleLite.java (revision 174) -+++ D:/project.files/java.Lab/NyARToolKit/sample/jogl/JavaSimpleLite.java (revision 178) -@@ -1,217 +0,0 @@ --/** -- * simpleLite‚Æ“¯‚¶‚悤‚ȃeƒXƒgƒvƒƒOƒ‰ƒ€ -- * ƒ}[ƒJ[‚̈ê’v“x‚̍Œá’l‚ðƒ`ƒFƒbƒN‚·‚é‚Æ‚±‚ë‚𔲂¢‚½‚̂ŁA“¯‚¶ƒ}[ƒJ[‚ð‘å—Ê‚É -- * ŒŸo‚·‚é‚Æ–Ê”’‚¢‚±‚Æ‚É‚È‚è‚Ü‚·B -- * (c)2008 R.iizuka -- * airmail(at)ebony.plala.or.jp -- * http://nyatla.jp/ -- */ --import java.awt.event.WindowAdapter; --import java.awt.event.WindowEvent; --import java.awt.*; -- --import javax.media.Buffer; -- --import javax.media.opengl.GL; --import javax.media.opengl.GLAutoDrawable; --import javax.media.opengl.GLEventListener; --import javax.media.opengl.GLCanvas; -- --import com.sun.opengl.util.Animator; -- --import jp.nyatla.nyartoolkit.core.NyARCode; -- --import jp.nyatla.nyartoolkit.jmf.JmfCameraCapture; --import jp.nyatla.nyartoolkit.jmf.JmfCaptureListener; --import jp.nyatla.nyartoolkit.gutil.*; -- -- --public class JavaSimpleLite implements GLEventListener,JmfCaptureListener --{ -- private final String CARCODE_FILE ="../../Data/patt.hiro"; -- private final String PARAM_FILE ="../../Data/camera_para.dat"; -- -- -- private Animator animator; -- private GLNyARRaster_RGB cap_image; -- -- private JmfCameraCapture capture; -- private GL gl; -- //NyARToolkitŠÖŒW -- private GLNyARSingleDetectMarker nya; -- private GLNyARParam ar_param; -- /** -- * —§•û‘Ì‚ð‘‚­ -- * -- */ -- void drawCube() -- { -- // Colour cube data. -- int polyList = 0; -- float fSize = 0.5f; -- int f, i; -- float[][] cube_vertices=new float[][]{ -- {1.0f, 1.0f, 1.0f}, {1.0f, -1.0f, 1.0f}, {-1.0f, -1.0f, 1.0f}, {-1.0f, 1.0f, 1.0f}, -- {1.0f, 1.0f, -1.0f}, {1.0f, -1.0f, -1.0f}, {-1.0f, -1.0f, -1.0f}, {-1.0f, 1.0f, -1.0f} -- }; -- float[][] cube_vertex_colors=new float[][]{ -- {1.0f, 1.0f, 1.0f}, {1.0f, 1.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 1.0f, 1.0f}, -- {1.0f, 0.0f, 1.0f}, {1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 1.0f} -- }; -- int cube_num_faces = 6; -- short[][] cube_faces =new short[][]{ -- {3, 2, 1, 0}, {2, 3, 7, 6}, {0, 1, 5, 4}, {3, 0, 4, 7}, {1, 2, 6, 5}, {4, 5, 6, 7} -- }; -- -- if (polyList==0) { -- polyList = gl.glGenLists (1); -- gl.glNewList(polyList, GL.GL_COMPILE); -- gl.glBegin(GL.GL_QUADS); -- for (f = 0; f < cube_num_faces; f++) -- for (i = 0; i < 4; i++) { -- gl.glColor3f (cube_vertex_colors[cube_faces[f][i]][0], cube_vertex_colors[cube_faces[f][i]][1], cube_vertex_colors[cube_faces[f][i]][2]); -- gl.glVertex3f(cube_vertices[cube_faces[f][i]][0] * fSize, cube_vertices[cube_faces[f][i]][1] * fSize, cube_vertices[cube_faces[f][i]][2] * fSize); -- } -- gl.glEnd(); -- gl.glColor3f(0.0f, 0.0f, 0.0f); -- for (f = 0; f < cube_num_faces; f++) { -- gl.glBegin (GL.GL_LINE_LOOP); -- for (i = 0; i < 4; i++) -- gl.glVertex3f(cube_vertices[cube_faces[f][i]][0] * fSize, cube_vertices[cube_faces[f][i]][1] * fSize, cube_vertices[cube_faces[f][i]][2] * fSize); -- gl.glEnd (); -- } -- gl.glEndList (); -- } -- -- gl.glPushMatrix(); // Save world coordinate system. -- gl.glTranslatef(0.0f, 0.0f, 0.5f); // Place base of cube on marker surface. -- gl.glRotatef(0.0f, 0.0f, 0.0f, 1.0f); // Rotate about z axis. -- gl.glDisable(GL.GL_LIGHTING); // Just use colours. -- gl.glCallList(polyList); // Draw the cube. -- gl.glPopMatrix(); // Restore world coordinate system. -- -- } -- -- -- -- public JavaSimpleLite() -- { -- Frame frame = new Frame("Java simpleLite with NyARToolkit"); -- -- -- // 3D‚ð•`‰æ‚·‚éƒRƒ“ƒ|[ƒlƒ“ƒg -- GLCanvas canvas = new GLCanvas(); -- frame.add(canvas); -- canvas.addGLEventListener(this); -- frame.addWindowListener(new WindowAdapter() { -- public void windowClosing(WindowEvent e) { -- System.exit(0); -- } -- }); -- -- frame.setVisible(true); -- Insets ins=frame.getInsets(); -- frame.setSize(320+ins.left+ins.right,240+ins.top+ins.bottom); -- canvas.setBounds(ins.left,ins.top,320,240); -- } -- -- public void init(GLAutoDrawable drawable) { -- gl = drawable.getGL(); -- gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f); -- //NyARToolkit‚̏€”õ -- try{ -- //ƒLƒƒƒvƒ`ƒƒ‚̏€”õ -- capture=new JmfCameraCapture(320,240,15f,JmfCameraCapture.PIXEL_FORMAT_RGB); -- capture.setCaptureListener(this); -- //NyARToolkit‚̏€”õ -- ar_param=new GLNyARParam(); -- NyARCode ar_code =new NyARCode(16,16); -- ar_param.loadFromARFile(PARAM_FILE); -- ar_param.changeSize(320,240); -- nya=new GLNyARSingleDetectMarker(ar_param,ar_code,80.0); -- ar_code.loadFromARFile(CARCODE_FILE); -- //GL‘Ήž‚ÌRGBƒ‰ƒXƒ^ƒIƒuƒWƒFƒNƒg -- cap_image=new GLNyARRaster_RGB(gl,ar_param); -- //ƒLƒƒƒvƒ`ƒƒŠJŽn -- capture.start(); -- }catch(Exception e){ -- e.printStackTrace(); -- } -- animator = new Animator(drawable); -- -- animator.start(); -- -- } -- -- public void reshape(GLAutoDrawable drawable, -- int x, int y, -- int width, int height) -- { -- float ratio = (float)height / (float)width; -- gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT); -- gl.glViewport(0, 0, width, height); -- -- //Ž‹‘̐ς̐ݒè -- gl.glMatrixMode(GL.GL_PROJECTION); -- gl.glLoadIdentity(); -- gl.glFrustum(-1.0f, 1.0f, -ratio, ratio, -- 5.0f, 40.0f); -- //Œ©‚éˆÊ’u -- gl.glMatrixMode(GL.GL_MODELVIEW); -- gl.glLoadIdentity(); -- gl.glTranslatef(0.0f, 0.0f, -10.0f); -- } -- -- public void display(GLAutoDrawable drawable) -- { -- -- try{ -- if(!cap_image.hasData()){ -- return; -- } -- gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT); // Clear the buffers for new frame. -- //‰æ‘œƒ`ƒFƒbƒN‚µ‚ă}[ƒJ[’T‚µ‚āA”wŒi‚ð‘‚­ -- boolean is_marker_exist; -- synchronized(cap_image){ -- is_marker_exist=nya.detectMarkerLite(cap_image,100); -- //”wŒi‚ð‘‚­ -- cap_image.glDispImage(1.0); -- } -- //‚ ‚Á‚½‚ç—§•û‘Ì‚ð‘‚­ -- if(is_marker_exist){ -- // Projection transformation. -- gl.glMatrixMode(GL.GL_PROJECTION); -- gl.glLoadMatrixd(ar_param.getCameraFrustumRH(),0); -- gl.glMatrixMode(GL.GL_MODELVIEW); -- // Viewing transformation. -- gl.glLoadIdentity(); -- gl.glLoadMatrixd(nya.getCameraViewRH(),0); -- -- -- // All other lighting and geometry goes here. -- drawCube(); -- } -- }catch(Exception e){ -- e.printStackTrace(); -- } -- } -- public void onUpdateBuffer(Buffer i_buffer) -- { -- try{ -- synchronized(cap_image){ -- cap_image.setBuffer(i_buffer, true); -- } -- }catch(Exception e){ -- e.printStackTrace(); -- } -- } -- -- public void displayChanged(GLAutoDrawable drawable, -- boolean modeChanged, -- boolean deviceChanged) {} -- -- public static void main(String[] args) { -- new JavaSimpleLite(); -- } --} -- -Index: D:/project.files/java.Lab/NyARToolKit/sample/jogl/jp/nyatla/nyartoolkit/jogl/utils/GLNyARRaster_RGB.java -=================================================================== ---- D:/project.files/java.Lab/NyARToolKit/sample/jogl/jp/nyatla/nyartoolkit/jogl/utils/GLNyARRaster_RGB.java (revision 0) -+++ D:/project.files/java.Lab/NyARToolKit/sample/jogl/jp/nyatla/nyartoolkit/jogl/utils/GLNyARRaster_RGB.java (revision 178) -@@ -0,0 +1,76 @@ -+/** -+ * NyARRaster_RGB‚ÉOpenGL—p‚̃f[ƒ^•ÏŠ·‹@”\‚ð’ljÁ‚µ‚½‚à‚Ì‚Å‚·B -+ * -+ * (c)2008 AŒÕ—nyatla.jp -+ * airmail(at)ebony.plala.or.jp -+ * http://nyatla.jp/ -+ */ -+package jp.nyatla.nyartoolkit.jogl.utils; -+ -+import javax.media.format.RGBFormat; -+import javax.media.opengl.GL; -+import jp.nyatla.nyartoolkit.NyARException; -+import jp.nyatla.nyartoolkit.jmf.utils.*; -+import jp.nyatla.nyartoolkit.core.*; -+ -+public class GLNyARRaster_RGB extends JmfNyARRaster_RGB -+{ -+ private byte[] gl_buf; -+ private int gl_flag; -+ -+ public GLNyARRaster_RGB(GL i_ref_gl,NyARParam i_param) -+ { -+ super(i_param.getX(),i_param.getY()); -+ this.gl_flag=GL.GL_RGB; -+ this.gl_buf=new byte[this.width*this.height*3]; -+ } -+ public void setBuffer(javax.media.Buffer i_buffer,boolean i_is_reverse) throws NyARException -+ { -+ //JMFƒf[ƒ^‚ŃtƒH[ƒ}ƒbƒgƒvƒƒpƒeƒB‚ð‰Šú‰» -+ initFormatProperty((RGBFormat)i_buffer.getFormat()); -+ -+ byte[] src_buf=(byte[])i_buffer.getData(); -+ //GL—p‚̃f[ƒ^‚ð€”õ -+ if(i_is_reverse){ -+ int length=this.width*3; -+ int src_idx=0; -+ int dest_idx=(this.height-1)*length; -+ for(int i=0;i - - - + + + diff --git a/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARRaster_RGB.java b/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARRaster_RGB.java index 73d2ba3..1974a57 100644 --- a/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARRaster_RGB.java +++ b/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/J3dNyARRaster_RGB.java @@ -18,47 +18,59 @@ import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.NyARParam; import jp.nyatla.nyartoolkit.jmf.utils.*; - +/** + * + * @author A虎＠ + * このクラスは、Java3Dと互換性のあるNyARToolkitのラスタイメージを保持します。 + * + */ public class J3dNyARRaster_RGB extends JmfNyARRaster_RGB { -// public ImageUpdater updater; private ImageComponent2D imc2d; private byte[] i2d_buf; + private BufferedImage bufferd_image; + /** + * JMFのキャプチャ画像をこのクラスのBufferedImageにコピーします。 + * @param i_buffer + * 画像の格納されたバッファを指定して下さい。 + * 画像サイズはコンストラクタで与えたパラメタと同じサイズである必要があります。 + */ public void setBuffer(javax.media.Buffer i_buffer) throws NyARException { super.setBuffer(i_buffer); //メモ：この時点では、ref_dataにはi_bufferの参照値が入ってる。 - synchronized(imc2d){ + synchronized(this){ //キャプチャデータをi2dのバッファにコピーする。（これ省略したいなあ…。） - System.arraycopy(ref_buf,0,i2d_buf,0,this.i2d_buf.length); + System.arraycopy(this.ref_buf,0,this.i2d_buf,0,this.i2d_buf.length); } //ここでref_bufの参照値をref_bufへ移動 - ref_buf=i2d_buf; + this.ref_buf=this.i2d_buf; } public J3dNyARRaster_RGB(NyARParam i_cparam) { super(i_cparam.getX(),i_cparam.getY()); //RGBのラスタを作る。 -// ColorSpace cs=ColorSpace.getInstance(ColorSpace.CS_LINEAR_RGB); -// ComponentColorModel cm; -// cm=new ComponentColorModel(cs,new int[]{8,8,8},false,false,ComponentColorModel.OPAQUE,DataBuffer.TYPE_BYTE); -// java.awt.image.WritableRaster raster=cm.createCompatibleWritableRaster(width,height); -// i2d_buf=((DataBufferByte)raster.getDataBuffer()).getData(); -// BufferedImage background_image = new BufferedImage(cm,raster, false, null); - BufferedImage background_image = new BufferedImage(width,height,BufferedImage.TYPE_3BYTE_BGR); - i2d_buf=((DataBufferByte)background_image.getRaster().getDataBuffer()).getData(); - // - imc2d= new ImageComponent2D(ImageComponent2D.FORMAT_RGB, background_image, true, true); - imc2d.setCapability(ImageComponent.ALLOW_IMAGE_WRITE); + this.bufferd_image = new BufferedImage(this.width,this.height,BufferedImage.TYPE_3BYTE_BGR); + i2d_buf=((DataBufferByte)bufferd_image.getRaster().getDataBuffer()).getData(); + this.imc2d= new ImageComponent2D(ImageComponent2D.FORMAT_RGB, this.bufferd_image, true, true); + imc2d.setCapability(ImageComponent.ALLOW_IMAGE_WRITE); } /** - * このオブジェクトと連動するImageComponent2Dオブジェクトの参照値を得る。 - * @return + * 自身の格納しているImageComponent2Dオブジェクトを作り直します。 + * Java3D1.5がDirectXで動いた（らしいとき）に、ImageComponent2Dのインスタンス + * IDが異ならないと、Behavior内でイメージの更新を通知できない事象に対応するために実装してあります。 + * Behavior内でgetImageComponent2()関数を実行する直前に呼び出すことで、この事象を回避することができます。 + * */ + public void renewImageComponent2D() + { + this.imc2d= new ImageComponent2D(ImageComponent2D.FORMAT_RGB, this.bufferd_image, true, true); + this.imc2d.setCapability(ImageComponent.ALLOW_IMAGE_WRITE); + } public ImageComponent2D getImageComponent2D() { - return imc2d; + return this.imc2d; } } diff --git a/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorHolder.java b/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorHolder.java index af3fc6f..56ed926 100644 --- a/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorHolder.java +++ b/sample/java3d/src/jp/nyatla/nyartoolkit/java3d/utils/NyARSingleMarkerBehaviorHolder.java @@ -147,7 +147,6 @@ class NyARBehavior extends Behavior listener=i_listener; } } - /** * いわゆるイベントハンドラ */ @@ -158,7 +157,8 @@ class NyARBehavior extends Behavior Transform3D t3d=null; boolean is_marker_exist=false; if(back_ground!=null){ - back_ground.setImage(raster.getImageComponent2D()); + raster.renewImageComponent2D();/*DirectXモードのときの対策*/ + back_ground.setImage(raster.getImageComponent2D()); } if(raster.hasData()){ is_marker_exist=related_nya.detectMarkerLite(raster, 100); diff --git a/sample/jmf/jp/nyatla/nyartoolkit/jmf/utils/JmfNyARRaster_RGB.java b/sample/jmf/jp/nyatla/nyartoolkit/jmf/utils/JmfNyARRaster_RGB.java index d36ebfa..9e9dd9e 100644 --- a/sample/jmf/jp/nyatla/nyartoolkit/jmf/utils/JmfNyARRaster_RGB.java +++ b/sample/jmf/jp/nyatla/nyartoolkit/jmf/utils/JmfNyARRaster_RGB.java @@ -81,8 +81,18 @@ public class JmfNyARRaster_RGB implements NyARRaster public int getPixelTotal(int i_x,int i_y) { int bp=(i_x+i_y*width)*3; - return (ref_buf[bp] & 0xff)+(ref_buf[bp+1] & 0xff)+(ref_buf[bp+2] & 0xff); + byte[] ref=this.ref_buf; + return (ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); } + public void getPixelTotalRowLine(int i_row,int[] o_line) + { + final byte[] ref=this.ref_buf; + int bp=(i_row+1)*this.width*3-3; + for(int i=this.width-1;i>=0;i--){ + o_line[i]=(ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); + bp-=3; + } + } public int getWidth() { return width; @@ -91,12 +101,13 @@ public class JmfNyARRaster_RGB implements NyARRaster { return height; } - public void pickRgbArray(int i_x,int i_y,int[] i_rgb) + public void getPixel(int i_x,int i_y,int[] i_rgb) { - int bp=(i_x+i_y*width)*3; - i_rgb[0]=(ref_buf[bp+red_idx] & 0xff);//R - i_rgb[1]=(ref_buf[bp+green_idx] & 0xff);//G - i_rgb[2]=(ref_buf[bp+blue_idx] & 0xff);//B + int bp=(i_x+i_y*this.width)*3; + byte[] ref=this.ref_buf; + i_rgb[0]=(ref[bp+this.red_idx] & 0xff);//R + i_rgb[1]=(ref[bp+this.green_idx] & 0xff);//G + i_rgb[2]=(ref[bp+this.blue_idx] & 0xff);//B } /** * ピクセルの順序タイプを返します。 @@ -115,5 +126,20 @@ public class JmfNyARRaster_RGB implements NyARRaster { return ref_buf!=null; } - + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb) + { + int ri=this.red_idx; + int bi=this.green_idx; + int gi=this.blue_idx; + int width=this.width; + byte[] ref=this.ref_buf; + int bp; + for(int i=i_num-1;i>=0;i--){ + bp=(i_x[i]+i_y[i]*width)*3; + o_rgb[i*3+0]=(ref[bp+ri] & 0xff);//R + o_rgb[i*3+1]=(ref[bp+gi] & 0xff);//G + o_rgb[i*3+2]=(ref[bp+bi] & 0xff);//B + } + return; + } } diff --git a/sample/jogl/jp/nyatla/nyartoolkit/jogl/utils/GLNyARSingleDetectMarker.java b/sample/jogl/jp/nyatla/nyartoolkit/jogl/utils/GLNyARSingleDetectMarker.java index 034e881..ddda998 100644 --- a/sample/jogl/jp/nyatla/nyartoolkit/jogl/utils/GLNyARSingleDetectMarker.java +++ b/sample/jogl/jp/nyatla/nyartoolkit/jogl/utils/GLNyARSingleDetectMarker.java @@ -15,7 +15,7 @@ import jp.nyatla.nyartoolkit.detector.*; public class GLNyARSingleDetectMarker extends NyARSingleDetectMarker { private double view_scale_factor=0.025;//#define VIEW_SCALEFACTOR 0.025 // 1.0 ARToolKit unit becomes 0.025 of my OpenGL units. - public GLNyARSingleDetectMarker(NyARParam i_param,NyARCode i_code,double i_marker_width) + public GLNyARSingleDetectMarker(NyARParam i_param,NyARCode i_code,double i_marker_width) throws NyARException { super(i_param,i_code,i_marker_width); } diff --git a/src/jp/nyatla/nyartoolkit/base/Param.java b/src/jp/nyatla/nyartoolkit/base/Param.java index 42a7368..270589d 100644 --- a/src/jp/nyatla/nyartoolkit/base/Param.java +++ b/src/jp/nyatla/nyartoolkit/base/Param.java @@ -126,12 +126,12 @@ public class Param { NyARMat.matrixTrans(mat_at, mat_a );//if( arMatrixTrans( mat_at, mat_a ) < 0 ){ NyARException.trap("未チェックのパス"); - NyARMat.matrixMul( mat_wm1, mat_at, mat_a );//if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) { + mat_wm1.matrixMul(mat_at, mat_a );//if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) { NyARException.trap("未チェックのパス"); mat_wm1.matrixSelfInv();//if( arMatrixSelfInv( mat_wm1 ) < 0 ) { NyARException.trap("未チェックのパス"); - NyARMat.matrixMul( mat_wm2, mat_wm1, mat_at );//if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) { + mat_wm2.matrixMul(mat_wm1, mat_at );//if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) { //mat_cpara.row = AR_PARAM_CDMIN-1;//mat_cpara.row = AR_PARAM_CDMIN-1; //mat_cpara.clm = 1; @@ -140,7 +140,7 @@ public class Param { NyARMat mat_cpara=new NyARMat(arParamGet_AR_PARAM_CDMIN-1,1); double[][] mat_cpara_array=mat_cpara.getArray(); NyARException.trap("未チェックのパス"); - NyARMat.matrixMul(mat_cpara, mat_wm2, mat_r );//if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) { + mat_cpara.matrixMul(mat_wm2, mat_r );//if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) { for(int i2=0;i<3;i++){ for(int i3=0;i3<4;i3++){ diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java index f2f1021..e49acc0 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java @@ -34,194 +34,13 @@ package jp.nyatla.nyartoolkit.core; import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.raster.*; -/** - * 24ビットカラーのマーカーを保持するために使うクラスです。 - * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 - * - */ -public class NyARColorPatt +public interface NyARColorPatt { - public static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 - private short extpat[][][]; - private int width; - private int height; - public NyARColorPatt(int i_width,int i_height) - { - width=i_width; - height=i_height; - extpat=new short[i_height][i_width][3]; - } - public short[][][] getPatArray() - { - return extpat; - } - public int getWidth() - { - return width; - } - public int getHeight() - { - return height; - } - private static void get_cpara( double world[][], double vertex[][],double para[][] ) throws NyARException - { - NyARMat a = new NyARMat( 8, 8 ); - double[][] a_array=a.getArray(); - NyARMat b = new NyARMat( 8, 1 ); - double[][] b_array=b.getArray(); - NyARMat c = new NyARMat( 8, 1 ); - double[][] c_array=c.getArray(); - - for(int i = 0; i < 4; i++ ) { - a_array[i*2][0]=world[i][0];//a->m[i*16+0] = world[i][0]; - a_array[i*2][1]=world[i][1];//a->m[i*16+1] = world[i][1]; - a_array[i*2][2]=1.0;//a->m[i*16+2] = 1.0; - a_array[i*2][3]=0.0;//a->m[i*16+3] = 0.0; - a_array[i*2][4]=0.0;//a->m[i*16+4] = 0.0; - a_array[i*2][5]=0.0;//a->m[i*16+5] = 0.0; - a_array[i*2][6]=-world[i][0] * vertex[i][0];//a->m[i*16+6] = -world[i][0] * vertex[i][0]; - a_array[i*2][7]=-world[i][1] * vertex[i][0];//a->m[i*16+7] = -world[i][1] * vertex[i][0]; - a_array[i*2+1][0]=0.0;//a->m[i*16+8] = 0.0; - a_array[i*2+1][1]=0.0;//a->m[i*16+9] = 0.0; - a_array[i*2+1][2]=0.0;//a->m[i*16+10] = 0.0; - a_array[i*2+1][3]=world[i][0];//a->m[i*16+11] = world[i][0]; - a_array[i*2+1][4]=world[i][1];//a->m[i*16+12] = world[i][1]; - a_array[i*2+1][5]=1.0;//a->m[i*16+13] = 1.0; - a_array[i*2+1][6]=-world[i][0] * vertex[i][1];//a->m[i*16+14] = -world[i][0] * vertex[i][1]; - a_array[i*2+1][7]=-world[i][1] * vertex[i][1];//a->m[i*16+15] = -world[i][1] * vertex[i][1]; - b_array[i*2+0][0]=vertex[i][0];//b->m[i*2+0] = vertex[i][0]; - b_array[i*2+1][0]=vertex[i][1];//b->m[i*2+1] = vertex[i][1]; - } -// JartkException.trap("未チェックのパス"); - a.matrixSelfInv(); - -// JartkException.trap("未チェックのパス"); - NyARMat.matrixMul( c, a, b ); - for(int i = 0; i < 2; i++ ) { - para[i][0] = c_array[i*3+0][0];//para[i][0] = c->m[i*3+0]; - para[i][1] = c_array[i*3+1][0];//para[i][1] = c->m[i*3+1]; - para[i][2] = c_array[i*3+2][0];//para[i][2] = c->m[i*3+2]; - } - para[2][0] = c_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; - para[2][1] = c_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; - para[2][2] = 1.0;//para[2][2] = 1.0; - } - /** - * imageから、i_markerの位置にあるパターンを切り出して、保持します。 - * @param image - * @param i_marker - * @throws Exception - */ - public void pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException - { - int[] x_coord=i_marker.x_coord; - int[] y_coord=i_marker.y_coord; - int[] vertex=i_marker.vertex; - int[][][] ext_pat2=new int[height][width][3];//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; - double[][] world=new double[4][2];//double world[4][2]; - double[][] local=new double[4][2];//double local[4][2]; - double[][] para=new double[3][3]; //double para[3][3]; - double d, xw, yw; - int xc, yc; - int xdiv, ydiv; - int xdiv2, ydiv2; - int lx1, lx2, ly1, ly2; - int img_x,img_y; - - img_x=image.getWidth(); - img_y=image.getHeight(); - - double xdiv2_reciprocal; // [tp] - double ydiv2_reciprocal; // [tp] - int ext_pat2_x_index; - int ext_pat2_y_index; - - world[0][0] = 100.0; - world[0][1] = 100.0; - world[1][0] = 100.0 + 10.0; - world[1][1] = 100.0; - world[2][0] = 100.0 + 10.0; - world[2][1] = 100.0 + 10.0; - world[3][0] = 100.0; - world[3][1] = 100.0 + 10.0; - for(int i = 0; i < 4; i++ ) { - local[i][0] = x_coord[vertex[i]]; - local[i][1] = y_coord[vertex[i]]; - } - get_cpara( world, local, para ); - - lx1 = (int)((local[0][0] - local[1][0])*(local[0][0] - local[1][0])+ (local[0][1] - local[1][1])*(local[0][1] - local[1][1])); - lx2 = (int)((local[2][0] - local[3][0])*(local[2][0] - local[3][0])+ (local[2][1] - local[3][1])*(local[2][1] - local[3][1])); - ly1 = (int)((local[1][0] - local[2][0])*(local[1][0] - local[2][0])+ (local[1][1] - local[2][1])*(local[1][1] - local[2][1])); - ly2 = (int)((local[3][0] - local[0][0])*(local[3][0] - local[0][0])+ (local[3][1] - local[0][1])*(local[3][1] - local[0][1])); - if( lx2 > lx1 ){ - lx1 = lx2; - } - if( ly2 > ly1 ){ - ly1 = ly2; - } - xdiv2 =width; - ydiv2 =height; - - while( xdiv2*xdiv2 < lx1/4 ){ - xdiv2*=2; - } - while( ydiv2*ydiv2 < ly1/4 ){ - ydiv2*=2; - } - - if( xdiv2 > AR_PATT_SAMPLE_NUM) - { - xdiv2 =AR_PATT_SAMPLE_NUM; - } - if( ydiv2 >AR_PATT_SAMPLE_NUM) - { - ydiv2 = AR_PATT_SAMPLE_NUM; - } - - xdiv = xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X; - ydiv = ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y; -/* -printf("%3d(%f), %3d(%f)\n", xdiv2, sqrt(lx1), ydiv2, sqrt(ly1)); -*/ - - xdiv2_reciprocal = 1.0 / xdiv2; - ydiv2_reciprocal = 1.0 / ydiv2; - int[] rgb_tmp=new int[3]; - // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) ); - for(int j = 0; j < ydiv2; j++ ) { - yw = 102.5 + 5.0 * (j+0.5) * ydiv2_reciprocal; - for(int i = 0; i < xdiv2; i++ ) { - xw = 102.5 + 5.0 * (i+0.5) * xdiv2_reciprocal; - d = para[2][0]*xw + para[2][1]*yw + para[2][2]; - if( d == 0 ){ - throw new NyARException(); - } - xc = (int)((para[0][0]*xw + para[0][1]*yw + para[0][2])/d); - yc = (int)((para[1][0]*xw + para[1][1]*yw + para[1][2])/d); - + public int getWidth(); + public int getHeight(); + public int[][][] getPatArray(); + public void pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException; +} - if( xc >= 0 && xc < img_x && yc >= 0 && yc < img_y ) { - ext_pat2_y_index = j/ydiv; - ext_pat2_x_index = i/xdiv; -// image_index = (yc*arUtil_c.arImXsize+xc)*Config.AR_PIX_SIZE_DEFAULT; - image.pickRgbArray(xc, yc, rgb_tmp); - ext_pat2[ext_pat2_y_index][ext_pat2_x_index][0] += rgb_tmp[0];//R - ext_pat2[ext_pat2_y_index][ext_pat2_x_index][1] += rgb_tmp[1];//G - ext_pat2[ext_pat2_y_index][ext_pat2_x_index][2] += rgb_tmp[2];//B -// System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]); - } - } - } -// short[][][] ext_pat=new short[Config.AR_PATT_SIZE_Y][Config.AR_PATT_SIZE_X][3];//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; - for(int j = 0; j < height; j++ ) { - for(int i = 0; i < width; i++ ) { // PRL 2006-06-08. - extpat[j][i][0]=(short)(ext_pat2[j][i][0] / (xdiv*ydiv));//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); - extpat[j][i][1]=(short)(ext_pat2[j][i][1] / (xdiv*ydiv));//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); - extpat[j][i][2]=(short)(ext_pat2[j][i][2] / (xdiv*ydiv));//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); - } - } - } -} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O1.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O1.java new file mode 100644 index 0000000..db47266 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O1.java @@ -0,0 +1,268 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.NyARRaster; + +/** + * 24ビットカラーのマーカーを保持するために使うクラスです。 + * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 + * 演算順序以外の最適化をしたもの + * + */ +public class NyARColorPatt_O1 implements NyARColorPatt +{ + private static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 + private int extpat[][][]; + private int width; + private int height; + public NyARColorPatt_O1(int i_width,int i_height) + { + this.width=i_width; + this.height=i_height; + this.extpat=new int[i_height][i_width][3]; + this.wk_pickFromRaster_ext_pat2=new int[i_height][i_width][3]; + } + public int[][][] getPatArray() + { + return extpat; + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + private final NyARMat wk_get_cpara_a=new NyARMat(8,8); + private final NyARMat wk_get_cpara_b=new NyARMat(8,1); + private final NyARMat wk_get_cpara_c=new NyARMat(8,1); + /** + * + * @param world + * @param vertex + * @param para + * [3x3] + * @throws NyARException + */ + private void get_cpara( double world[][], double vertex[][],double[] para) throws NyARException + { + NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 ); + double[][] a_array=a.getArray(); + NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 ); + double[][] b_array=b.getArray(); + double[] a_pt0,a_pt1,world_pti; + + for(int i = 0; i < 4; i++ ) { + a_pt0=a_array[i*2]; + a_pt1=a_array[i*2+1]; + world_pti=world[i]; + + a_pt0[0]=world_pti[0];//a->m[i*16+0] = world[i][0]; + a_pt0[1]=world_pti[1];//a->m[i*16+1] = world[i][1]; + a_pt0[2]=1.0;//a->m[i*16+2] = 1.0; + a_pt0[3]=0.0;//a->m[i*16+3] = 0.0; + a_pt0[4]=0.0;//a->m[i*16+4] = 0.0; + a_pt0[5]=0.0;//a->m[i*16+5] = 0.0; + a_pt0[6]=-world_pti[0] * vertex[i][0];//a->m[i*16+6] = -world[i][0] * vertex[i][0]; + a_pt0[7]=-world_pti[1] * vertex[i][0];//a->m[i*16+7] = -world[i][1] * vertex[i][0]; + a_pt1[0]=0.0;//a->m[i*16+8] = 0.0; + a_pt1[1]=0.0;//a->m[i*16+9] = 0.0; + a_pt1[2]=0.0;//a->m[i*16+10] = 0.0; + a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0]; + a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1]; + a_pt1[5]=1.0;//a->m[i*16+13] = 1.0; + a_pt1[6]=-world_pti[0] * vertex[i][1];//a->m[i*16+14] = -world[i][0] * vertex[i][1]; + a_pt1[7]=-world_pti[1] * vertex[i][1];//a->m[i*16+15] = -world[i][1] * vertex[i][1]; + b_array[i*2+0][0]=vertex[i][0];//b->m[i*2+0] = vertex[i][0]; + b_array[i*2+1][0]=vertex[i][1];//b->m[i*2+1] = vertex[i][1]; + } +// JartkException.trap("未チェックのパス"); + a.matrixSelfInv(); + +// JartkException.trap("未チェックのパス"); + NyARMat c = wk_get_cpara_c;//次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 ); + double[][] c_array=c.getArray(); + + c.matrixMul(a, b); + for(int i = 0; i < 2; i++ ) { + para[i*3+0] = c_array[i*3+0][0];//para[i][0] = c->m[i*3+0]; + para[i*3+1] = c_array[i*3+1][0];//para[i][1] = c->m[i*3+1]; + para[i*3+2] = c_array[i*3+2][0];//para[i][2] = c->m[i*3+2]; + } + para[2*3+0] = c_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; + para[2*3+1] = c_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; + para[2*3+2] = 1.0;//para[2][2] = 1.0; + } + + private final double[][] wk_pickFromRaster_local=new double[4][2]; + private final double[] wk_pickFromRaster_para=new double[9];//[3][3]; + private int[][][] wk_pickFromRaster_ext_pat2=null;//コンストラクタでint[height][width][3]を作る + private final double[][] wk_pickFromRaster_world={//double world[4][2]; + {100.0, 100.0}, + {100.0+10.0,100.0}, + {100.0+10.0,100.0 + 10.0}, + {100.0, 100.0 + 10.0} + }; + /** + * pickFromRaster関数から使う変数です。 + * + */ + private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height) + { + int i,i2; + int[][] pt2; + int[] pt1; + for(i=i_height-1;i>=0;i--){ + pt2=i_ext_pat2[i]; + for(i2=i_width-1;i2>=0;i2--){ + pt1=pt2[i2]; + pt1[0]=0; + pt1[1]=0; + pt1[2]=0; + } + } + } + private final int[] wk_pickFromRaster_rgb_tmp=new int[3]; + /** + * imageから、i_markerの位置にあるパターンを切り出して、保持します。 + * Optimize:STEP[769->] + * @param image + * @param i_marker + * @throws Exception + */ + public void pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException + { + double d, xw, yw; + int xc, yc; + int xdiv, ydiv; + int xdiv2, ydiv2; + int lx1, lx2, ly1, ly2; + + int img_x=image.getWidth(); + int img_y=image.getHeight(); + + double xdiv2_reciprocal; // [tp] + double ydiv2_reciprocal; // [tp] + + + int[] x_coord=i_marker.x_coord; + int[] y_coord=i_marker.y_coord; + double[][] local=wk_pickFromRaster_local;//double local[4][2]; + int[] vertex=i_marker.mkvertex; + for(int i = 0; i < 4; i++ ) { + local[i][0] = x_coord[vertex[i]]; + local[i][1] = y_coord[vertex[i]]; + } + double[][] world=wk_pickFromRaster_world; +/* world[0][0] = 100.0; + world[0][1] = 100.0; + world[1][0] = 100.0 + 10.0; + world[1][1] = 100.0; + world[2][0] = 100.0 + 10.0; + world[2][1] = 100.0 + 10.0; + world[3][0] = 100.0; + world[3][1] = 100.0 + 10.0;*/ + double[] para =wk_pickFromRaster_para; //double para[3][3]; + get_cpara( world, local, para ); + lx1 = (int)((local[0][0] - local[1][0])*(local[0][0] - local[1][0])+ (local[0][1] - local[1][1])*(local[0][1] - local[1][1])); + lx2 = (int)((local[2][0] - local[3][0])*(local[2][0] - local[3][0])+ (local[2][1] - local[3][1])*(local[2][1] - local[3][1])); + ly1 = (int)((local[1][0] - local[2][0])*(local[1][0] - local[2][0])+ (local[1][1] - local[2][1])*(local[1][1] - local[2][1])); + ly2 = (int)((local[3][0] - local[0][0])*(local[3][0] - local[0][0])+ (local[3][1] - local[0][1])*(local[3][1] - local[0][1])); + if( lx2 > lx1 ){ + lx1 = lx2; + } + if( ly2 > ly1 ){ + ly1 = ly2; + } + xdiv2 =this.width; + ydiv2 =this.height; + + while( xdiv2*xdiv2 < lx1/4 ){ + xdiv2*=2; + } + while( ydiv2*ydiv2 < ly1/4 ){ + ydiv2*=2; + } + + if( xdiv2 > AR_PATT_SAMPLE_NUM) + { + xdiv2 =AR_PATT_SAMPLE_NUM; + } + if( ydiv2 >AR_PATT_SAMPLE_NUM) + { + ydiv2 = AR_PATT_SAMPLE_NUM; + } + + xdiv = xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X; + ydiv = ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y; + + /*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/ + int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; + int extpat_j[][],extpat_j_i[]; + int ext_pat2_j[][],ext_pat2_j_i[]; + + initValue_wk_pickFromRaster_ext_pat2(ext_pat2,this.width,this.height); + + xdiv2_reciprocal = 1.0 / xdiv2; + ydiv2_reciprocal = 1.0 / ydiv2; + int i,j; + int[] rgb_tmp=wk_pickFromRaster_rgb_tmp; + // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) ); + for(j = 0; j < ydiv2; j++ ) { + yw = 102.5 + 5.0 * (j+0.5) * ydiv2_reciprocal; + for(i = 0; i < xdiv2; i++ ) { + xw = 102.5 + 5.0 * (i+0.5) * xdiv2_reciprocal; + d = para[2*3+0]*xw + para[2*3+1]*yw + para[2*3+2]; + if( d == 0 ){ + throw new NyARException(); + } + xc = (int)((para[0*3+0]*xw + para[0*3+1]*yw + para[0*3+2])/d); + yc = (int)((para[1*3+0]*xw + para[1*3+1]*yw + para[1*3+2])/d); + + + if( xc >= 0 && xc < img_x && yc >= 0 && yc < img_y ) { + image.getPixel(xc, yc, rgb_tmp); + ext_pat2_j_i=ext_pat2[j/ydiv][i/xdiv]; + + ext_pat2_j_i[0] += rgb_tmp[0];//R + ext_pat2_j_i[1] += rgb_tmp[1];//G + ext_pat2_j_i[2] += rgb_tmp[2];//B +// System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]); + } + } + } +// short[][][] ext_pat=new short[Config.AR_PATT_SIZE_Y][Config.AR_PATT_SIZE_X][3];//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; + /**/ + int xdiv_x_ydiv=xdiv*ydiv; + for(j = this.height-1; j>=0; j--){ + extpat_j=extpat[j]; + ext_pat2_j=ext_pat2[j]; + for(i = this.width-1; i>=0; i--){ // PRL 2006-06-08. + ext_pat2_j_i=ext_pat2_j[i]; + extpat_j_i=extpat_j[i]; + extpat_j_i[0]=(ext_pat2_j_i[0] / xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_j_i[1]=(ext_pat2_j_i[1] / xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_j_i[2]=(ext_pat2_j_i[2] / xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + }/* + int xdiv_mul_ydiv=xdiv*ydiv; + short [][] extpat_pt_2; + short[] extpat_pt_1; + int[][] ext_pat2_pt_2; + int[] ext_pat2_pt_1; + for(int j=this.height-1; j>=0; j--){//for(int j = 0; j < this.height; j++ ){ + extpat_pt_2=extpat[j]; + ext_pat2_pt_2=ext_pat2[j]; + for(int i = this.width-1; i>=0; i--){//for(int i = 0; i < this.width; i++ ){ // PRL 2006-06-08. + extpat_pt_1=extpat_pt_2[i]; + ext_pat2_pt_1=ext_pat2_pt_2[i]; + extpat_pt_1[0]=(short)(ext_pat2_pt_1[0] / xdiv_mul_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_pt_1[1]=(short)(ext_pat2_pt_1[1] / xdiv_mul_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_pt_1[2]=(short)(ext_pat2_pt_1[2] / xdiv_mul_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + } + /**/ + return; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java new file mode 100644 index 0000000..0cea61b --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java @@ -0,0 +1,281 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.NyARRaster; + +/** + * 24ビットカラーのマーカーを保持するために使うクラスです。 + * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 + * 演算順序を含む最適化をしたもの + * + */ +public class NyARColorPatt_O2 implements NyARColorPatt +{ + private static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 + private int extpat[][][]; + private int width; + private int height; + public NyARColorPatt_O2(int i_width,int i_height) + { + this.width=i_width; + this.height=i_height; + this.extpat=new int[i_height][i_width][3]; + this.wk_pickFromRaster_ext_pat2=new int[i_height][i_width][3]; + } + public int[][][] getPatArray() + { + return extpat; + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + private final NyARMat wk_get_cpara_a=new NyARMat(8,8); + private final NyARMat wk_get_cpara_b=new NyARMat(8,1); +// private final NyARMat wk_get_cpara_c=new NyARMat(8,1); + /** + * + * @param world + * @param vertex + * @param para + * [3x3] + * @throws NyARException + */ + /** + * @param world + * @param vertex + * @param o_para + * @throws NyARException + */ + private void get_cpara(double vertex_0[], double vertex_1[],NyARMat o_para) throws NyARException + { + double world[][]=this.wk_pickFromRaster_world; + NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 ); + double[][] a_array=a.getArray(); + NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 ); + double[][] b_array=b.getArray(); + double[] a_pt0,a_pt1,world_pti; + + for(int i = 0; i < 4; i++ ) { + a_pt0=a_array[i*2]; + a_pt1=a_array[i*2+1]; + world_pti=world[i]; + + a_pt0[0]=world_pti[0];//a->m[i*16+0] = world[i][0]; + a_pt0[1]=world_pti[1];//a->m[i*16+1] = world[i][1]; + a_pt0[2]=1.0;//a->m[i*16+2] = 1.0; + a_pt0[3]=0.0;//a->m[i*16+3] = 0.0; + a_pt0[4]=0.0;//a->m[i*16+4] = 0.0; + a_pt0[5]=0.0;//a->m[i*16+5] = 0.0; + a_pt0[6]=-world_pti[0] * vertex_0[i];//a->m[i*16+6] = -world[i][0] * vertex[i][0]; + a_pt0[7]=-world_pti[1] * vertex_0[i];//a->m[i*16+7] = -world[i][1] * vertex[i][0]; + a_pt1[0]=0.0;//a->m[i*16+8] = 0.0; + a_pt1[1]=0.0;//a->m[i*16+9] = 0.0; + a_pt1[2]=0.0;//a->m[i*16+10] = 0.0; + a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0]; + a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1]; + a_pt1[5]=1.0;//a->m[i*16+13] = 1.0; + a_pt1[6]=-world_pti[0] * vertex_1[i];//a->m[i*16+14] = -world[i][0] * vertex[i][1]; + a_pt1[7]=-world_pti[1] * vertex_1[i];//a->m[i*16+15] = -world[i][1] * vertex[i][1]; + b_array[i*2+0][0]=vertex_0[i];//b->m[i*2+0] = vertex[i][0]; + b_array[i*2+1][0]=vertex_1[i];//b->m[i*2+1] = vertex[i][1]; + } +// JartkException.trap("未チェックのパス"); + a.matrixSelfInv(); + +// JartkException.trap("未チェックのパス"); +// NyARMat c = wk_get_cpara_c;//次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 ); +// double[][] c_array=c.getArray(); + + o_para.matrixMul(a, b); +// para[0*3+0] = c_array[0*3+0][0];//para[i][0] = c->m[i*3+0]; +// para[0*3+1] = c_array[0*3+1][0];//para[i][1] = c->m[i*3+1]; +// para[0*3+2] = c_array[0*3+2][0];//para[i][2] = c->m[i*3+2]; +// para[1*3+0] = c_array[1*3+0][0];//para[i][0] = c->m[i*3+0]; +// para[1*3+1] = c_array[1*3+1][0];//para[i][1] = c->m[i*3+1]; +// para[i*3+2] = c_array[1*3+2][0];//para[i][2] = c->m[i*3+2]; +// para[2*3+0] = c_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; +// para[2*3+1] = c_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; +// para[2*3+2] = 1.0;//para[2][2] = 1.0; + + + } + + // private final double[] wk_pickFromRaster_para=new double[9];//[3][3]; + private int[][][] wk_pickFromRaster_ext_pat2=null;//コンストラクタでint[height][width][3]を作る + private final double[][] wk_pickFromRaster_world={//double world[4][2]; + {100.0, 100.0}, + {100.0+10.0,100.0}, + {100.0+10.0,100.0 + 10.0}, + {100.0, 100.0 + 10.0} + }; + /** + * pickFromRaster関数から使う変数です。 + * + */ + private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height) + { + int i,i2; + int[][] pt2; + int[] pt1; + for(i=i_height-1;i>=0;i--){ + pt2=i_ext_pat2[i]; + for(i2=i_width-1;i2>=0;i2--){ + pt1=pt2[i2]; + pt1[0]=0; + pt1[1]=0; + pt1[2]=0; + } + } + } + private final double[][] wk_pickFromRaster_local=new double[2][4]; + private final int[] wk_pickFromRaster_rgb_tmp=new int[3]; + private final NyARMat wk_pickFromRaster_cpara=new NyARMat(8,1); + /** + * imageから、i_markerの位置にあるパターンを切り出して、保持します。 + * Optimize:STEP[769->750] + * @param image + * @param i_marker + * @throws Exception + */ + public void pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException + { + NyARMat cpara=this.wk_pickFromRaster_cpara; + //localの計算 + int[] x_coord=i_marker.x_coord; + int[] y_coord=i_marker.y_coord; + int[] vertex=i_marker.mkvertex; + double[] local_0=wk_pickFromRaster_local[0];//double local[4][2]; + double[] local_1=wk_pickFromRaster_local[1];//double local[4][2]; + for(int i = 0; i < 4; i++ ) { + local_0[i] = x_coord[vertex[i]]; + local_1[i] = y_coord[vertex[i]]; + } + //xdiv2,ydiv2の計算 + int xdiv2, ydiv2; + int l1,l2; + double w1,w2; + + //x計算 + w1=local_0[0] - local_0[1]; + w2=local_1[0] - local_1[1]; + l1 = (int)(w1*w1+w2*w2); + w1=local_0[2] - local_0[3]; + w2=local_1[2] - local_1[3]; + l2 = (int)(w1*w1+w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + l1=l1/4; + xdiv2 =this.width; + while( xdiv2*xdiv2 < l1 ){ + xdiv2*=2; + } + if( xdiv2 > AR_PATT_SAMPLE_NUM) + { + xdiv2 =AR_PATT_SAMPLE_NUM; + } + + //y計算 + w1=local_0[1] - local_0[2]; + w2=local_1[1] - local_1[2]; + l1 = (int)(w1*w1+ w2*w2); + w1=local_0[3] - local_0[0]; + w2=local_1[3] - local_1[0]; + l2 = (int)(w1*w1+ w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + ydiv2 =this.height; + l1=l1/4; + while( ydiv2*ydiv2 < l1 ){ + ydiv2*=2; + } + if( ydiv2 >AR_PATT_SAMPLE_NUM) + { + ydiv2 = AR_PATT_SAMPLE_NUM; + } + + //cparaの計算 + get_cpara(local_0,local_1,cpara); + + int img_x=image.getWidth(); + int img_y=image.getHeight(); + + /*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/ + int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; + int extpat_j[][],extpat_j_i[]; + int ext_pat2_j[][],ext_pat2_j_i[]; + + initValue_wk_pickFromRaster_ext_pat2(ext_pat2,this.width,this.height); + + double[][] cpara_array=cpara.getArray(); + double para21_x_yw,para01_x_yw,para11_x_yw; + double para00,para01,para02,para10,para11,para12,para20,para21; + para00 = cpara_array[0*3+0][0];//para[i][0] = c->m[i*3+0]; + para01 = cpara_array[0*3+1][0];//para[i][1] = c->m[i*3+1]; + para02 = cpara_array[0*3+2][0];//para[i][2] = c->m[i*3+2]; + para10 = cpara_array[1*3+0][0];//para[i][0] = c->m[i*3+0]; + para11 = cpara_array[1*3+1][0];//para[i][1] = c->m[i*3+1]; + para12 = cpara_array[1*3+2][0];//para[i][2] = c->m[i*3+2]; + para20 = cpara_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; + para21 = cpara_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; + //para22 = 1.0;//para[2][2] = 1.0; + + + double d, xw, yw; + int xc, yc; + int i,j; + int[] rgb_tmp=wk_pickFromRaster_rgb_tmp; + // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) ); + int xdiv = xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X; + int ydiv = ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y; + double xdiv2_reciprocal = 1.0 / xdiv2; + double ydiv2_reciprocal = 1.0 / ydiv2; + + for(j = 0; j < ydiv2; j++ ) { + yw = 102.5 + 5.0 * ((double)j+0.5) * ydiv2_reciprocal; + para21_x_yw=para21*yw+1.0; + para11_x_yw=para11*yw+para12; + para01_x_yw=para01*yw+para02; + ext_pat2_j=ext_pat2[j/ydiv]; + for(i = 0; i < xdiv2; i++ ) { + xw = 102.5 + 5.0 * ((double)i+0.5) * xdiv2_reciprocal; + d = para20*xw + para21_x_yw; + if( d == 0 ){ + throw new NyARException(); + } + xc = (int)((para00*xw + para01_x_yw)/d); + yc = (int)((para10*xw + para11_x_yw)/d); + + + if( xc >= 0 && xc < img_x && yc >= 0 && yc < img_y ) { + image.getPixel(xc, yc, rgb_tmp); + ext_pat2_j_i=ext_pat2_j[i/xdiv]; + + ext_pat2_j_i[0] += rgb_tmp[0];//R + ext_pat2_j_i[1] += rgb_tmp[1];//G + ext_pat2_j_i[2] += rgb_tmp[2];//B + } + } + } + /**/ + int xdiv_x_ydiv=xdiv*ydiv; + for(j =this.height-1; j>=0; j--){ + extpat_j=extpat[j]; + ext_pat2_j=ext_pat2[j]; + for(i = this.width-1; i>=0; i--){ // PRL 2006-06-08. + ext_pat2_j_i=ext_pat2_j[i]; + extpat_j_i=extpat_j[i]; + extpat_j_i[0]=(ext_pat2_j_i[0] / xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_j_i[1]=(ext_pat2_j_i[1] / xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_j_i[2]=(ext_pat2_j_i[2] / xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + } + return; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O3.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O3.java new file mode 100644 index 0000000..b56687e --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O3.java @@ -0,0 +1,340 @@ +package jp.nyatla.nyartoolkit.core; + + + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.NyARRaster; + +/** + * 24ビットカラーのマーカーを保持するために使うクラスです。 + * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 + * 演算順序を含む最適化をしたもの + * + */ +public class NyARColorPatt_O3 implements NyARColorPatt +{ + private static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 + private int extpat[][][]; + private int width; + private int height; + public NyARColorPatt_O3(int i_width,int i_height) + { + this.width=i_width; + this.height=i_height; + this.extpat=new int[i_height][i_width][3]; + } + public int[][][] getPatArray() + { + return extpat; + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + private final NyARMat wk_get_cpara_a=new NyARMat(8,8); + private final NyARMat wk_get_cpara_b=new NyARMat(8,1); + + /** + * + * @param world + * @param vertex + * @param para + * [3x3] + * @throws NyARException + */ + /** + * @param world + * @param vertex + * @param o_para + * @throws NyARException + */ + private void get_cpara(double vertex_0[], double vertex_1[],NyARMat o_para) throws NyARException + { + double world[][]=this.wk_pickFromRaster_world; + NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 ); + double[][] a_array=a.getArray(); + NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 ); + double[][] b_array=b.getArray(); + double[] a_pt0,a_pt1,world_pti; + + for(int i = 0; i < 4; i++ ) { + a_pt0=a_array[i*2]; + a_pt1=a_array[i*2+1]; + world_pti=world[i]; + + a_pt0[0]=world_pti[0];//a->m[i*16+0] = world[i][0]; + a_pt0[1]=world_pti[1];//a->m[i*16+1] = world[i][1]; + a_pt0[2]=1.0;//a->m[i*16+2] = 1.0; + a_pt0[3]=0.0;//a->m[i*16+3] = 0.0; + a_pt0[4]=0.0;//a->m[i*16+4] = 0.0; + a_pt0[5]=0.0;//a->m[i*16+5] = 0.0; + a_pt0[6]=-world_pti[0] * vertex_0[i];//a->m[i*16+6] = -world[i][0] * vertex[i][0]; + a_pt0[7]=-world_pti[1] * vertex_0[i];//a->m[i*16+7] = -world[i][1] * vertex[i][0]; + a_pt1[0]=0.0;//a->m[i*16+8] = 0.0; + a_pt1[1]=0.0;//a->m[i*16+9] = 0.0; + a_pt1[2]=0.0;//a->m[i*16+10] = 0.0; + a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0]; + a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1]; + a_pt1[5]=1.0;//a->m[i*16+13] = 1.0; + a_pt1[6]=-world_pti[0] * vertex_1[i];//a->m[i*16+14] = -world[i][0] * vertex[i][1]; + a_pt1[7]=-world_pti[1] * vertex_1[i];//a->m[i*16+15] = -world[i][1] * vertex[i][1]; + b_array[i*2+0][0]=vertex_0[i];//b->m[i*2+0] = vertex[i][0]; + b_array[i*2+1][0]=vertex_1[i];//b->m[i*2+1] = vertex[i][1]; + } + a.matrixSelfInv(); + + + o_para.matrixMul(a, b); + + + + } + + // private final double[] wk_pickFromRaster_para=new double[9];//[3][3]; + private final double[][] wk_pickFromRaster_world={//double world[4][2]; + {100.0, 100.0}, + {100.0+10.0,100.0}, + {100.0+10.0,100.0 + 10.0}, + {100.0, 100.0 + 10.0} + }; + /** + * pickFromRaster関数から使う変数です。 + * + */ + private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height) + { + int i,i2; + int[][] pt2; + int[] pt1; + for(i=i_height-1;i>=0;i--){ + pt2=i_ext_pat2[i]; + for(i2=i_width-1;i2>=0;i2--){ + pt1=pt2[i2]; + pt1[0]=0; + pt1[1]=0; + pt1[2]=0; + } + } + } + private final double[][] wk_pickFromRaster_local=new double[2][4]; + private final NyARMat wk_pickFromRaster_cpara=new NyARMat(8,1); + /** + * imageから、i_markerの位置にあるパターンを切り出して、保持します。 + * Optimize:STEP[769->750] + * @param image + * @param i_marker + * @throws Exception + */ + public void pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException + { + NyARMat cpara=this.wk_pickFromRaster_cpara; + //localの計算 + int[] x_coord=i_marker.x_coord; + int[] y_coord=i_marker.y_coord; + int[] vertex=i_marker.mkvertex; + double[] local_0=wk_pickFromRaster_local[0];//double local[4][2]; + double[] local_1=wk_pickFromRaster_local[1];//double local[4][2]; + for(int i = 0; i < 4; i++ ) { + local_0[i] = x_coord[vertex[i]]; + local_1[i] = y_coord[vertex[i]]; + } + //xdiv2,ydiv2の計算 + int xdiv2, ydiv2; + int l1,l2; + double w1,w2; + + //x計算 + w1=local_0[0] - local_0[1]; + w2=local_1[0] - local_1[1]; + l1 = (int)(w1*w1+w2*w2); + w1=local_0[2] - local_0[3]; + w2=local_1[2] - local_1[3]; + l2 = (int)(w1*w1+w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + l1=l1/4; + xdiv2 =this.width; + while( xdiv2*xdiv2 < l1 ){ + xdiv2*=2; + } + if( xdiv2 > AR_PATT_SAMPLE_NUM) + { + xdiv2 =AR_PATT_SAMPLE_NUM; + } + + //y計算 + w1=local_0[1] - local_0[2]; + w2=local_1[1] - local_1[2]; + l1 = (int)(w1*w1+ w2*w2); + w1=local_0[3] - local_0[0]; + w2=local_1[3] - local_1[0]; + l2 = (int)(w1*w1+ w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + ydiv2 =this.height; + l1=l1/4; + while( ydiv2*ydiv2 < l1 ){ + ydiv2*=2; + } + if( ydiv2 >AR_PATT_SAMPLE_NUM) + { + ydiv2 = AR_PATT_SAMPLE_NUM; + } + + //cparaの計算 + get_cpara(local_0,local_1,cpara); + updateExtpat(image,cpara,xdiv2,ydiv2); + + return; + } + //かなり大きいワークバッファを取るな…。 + private double[] wk_updateExtpat_para00_xw; + private double[] wk_updateExtpat_para10_xw; + private double[] wk_updateExtpat_para20_xw; + private int[] wk_updateExtpat_rgb_buf; + private int[] wk_updateExtpat_x_rgb_index; + private int[] wk_updateExtpat_y_rgb_index; + private int[] wk_updateExtpat_i_rgb_index; + + /** + * ワークバッファを予約する + * @param i_xdiv2 + */ + private void reservWorkBuffers(int i_xdiv2) + { + int[] buf=this.wk_updateExtpat_rgb_buf; + if(this.wk_updateExtpat_rgb_buf==null){ + wk_updateExtpat_para00_xw=new double[i_xdiv2]; + wk_updateExtpat_para10_xw=new double[i_xdiv2]; + wk_updateExtpat_para20_xw=new double[i_xdiv2]; + wk_updateExtpat_rgb_buf=new int[i_xdiv2*3]; + wk_updateExtpat_x_rgb_index=new int[i_xdiv2]; + wk_updateExtpat_y_rgb_index=new int[i_xdiv2]; + wk_updateExtpat_i_rgb_index=new int[i_xdiv2]; + }else{ + if(buf.lengthm[i*3+0]; + para01 = cpara_array[0*3+1][0];//para[i][1] = c->m[i*3+1]; + para02 = cpara_array[0*3+2][0];//para[i][2] = c->m[i*3+2]; + para10 = cpara_array[1*3+0][0];//para[i][0] = c->m[i*3+0]; + para11 = cpara_array[1*3+1][0];//para[i][1] = c->m[i*3+1]; + para12 = cpara_array[1*3+2][0];//para[i][2] = c->m[i*3+2]; + para20 = cpara_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; + para21 = cpara_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; + + + double d,yw; + int xc, yc; + int i,j; + // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) ); + int xdiv = i_xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X; + int ydiv = i_ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y; + + //計算バッファを予約する + this.reservWorkBuffers(i_xdiv2); + double[] para00_xw=this.wk_updateExtpat_para00_xw; + double[] para10_xw=this.wk_updateExtpat_para10_xw; + double[] para20_xw=this.wk_updateExtpat_para20_xw; + int[] x_rgb_index=this.wk_updateExtpat_x_rgb_index; + int[] y_rgb_index=this.wk_updateExtpat_y_rgb_index; + int[] i_rgb_index=this.wk_updateExtpat_i_rgb_index; + int[] rgb_buf=this.wk_updateExtpat_rgb_buf; + double xw; + for(i=0;i=img_x || yc<0 || yc >=img_y){ + continue; + } +// ピクセル値の計算 +// image.getPixel(xc,yc,rgb_buf); +// ext_pat2_j_i=ext_pat2_j[i/xdiv]; +// ext_pat2_j_i[0] += rgb_buf[0];//R +// ext_pat2_j_i[1] += rgb_buf[1];//G +// ext_pat2_j_i[2] += rgb_buf[2];//B + + x_rgb_index[index_num]=xc; + y_rgb_index[index_num]=yc; + i_rgb_index[index_num]=i/xdiv; + index_num++; + } +// //ステップ２．ピクセル配列を取得 + image.getPixelSet(x_rgb_index,y_rgb_index,index_num,rgb_buf); +// //ピクセル値の計算 + for(i=index_num-1;i>=0;i--){ + extpat_j_i=extpat_j[i_rgb_index[i]]; + extpat_j_i[0] += rgb_buf[i*3+0];//R + extpat_j_i[1] += rgb_buf[i*3+1];//G + extpat_j_i[2] += rgb_buf[i*3+2];//B + } + } + /**/ + int xdiv_x_ydiv=xdiv*ydiv; + for(j =this.height-1; j>=0; j--){ + extpat_j=extpat[j]; + for(i = this.width-1; i>=0; i--){ // PRL 2006-06-08. + extpat_j_i=extpat_j[i]; + extpat_j_i[0]/=(xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_j_i[1]/=(xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_j_i[2]/=(xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + } + return; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARDetectMarker.java b/src/jp/nyatla/nyartoolkit/core/NyARDetectMarker.java index 3d7370d..14a3baa 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARDetectMarker.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARDetectMarker.java @@ -35,7 +35,10 @@ package jp.nyatla.nyartoolkit.core; import jp.nyatla.nyartoolkit.NyARException; -import jp.nyatla.util.IntValue; + + + + /** * イメージからマーカー情報を検出するクラス。 * このクラスは、arDetectMarker2.cとの置き換えになります。 @@ -46,375 +49,254 @@ public class NyARDetectMarker { private static final int AR_AREA_MAX=100000;//#define AR_AREA_MAX 100000 private static final int AR_AREA_MIN=70;//#define AR_AREA_MIN 70 - private int area_max=AR_AREA_MAX; - private int area_min=AR_AREA_MIN; - private NyARMarker[] marker_holder; //マーカーデータの保持配列 - private NyARMarker[] marker_info2_array; //マーカーデータのインデックス配列 - private int marker_num; +// private final NyARMarker[] marker_holder; //マーカーデータの保持配列 +// private final NyARMarker[] marker_info2_array;//マーカーデータのインデックス配列 +// private int marker_num; private int width,height; /** * 最大i_squre_max個のマーカーを検出するクラスを作成する。 * @param i_width * @param i_height - * @param i_squre_max */ - public NyARDetectMarker(int i_width,int i_height,int i_squre_max) - { - width =i_width; - height=i_height; - marker_holder=new NyARMarker[i_squre_max]; - marker_info2_array=new NyARMarker[i_squre_max]; - } - public int getMarkerNum() + public NyARDetectMarker(int i_width,int i_height) { - return marker_num; - } - public NyARMarker getMarker(int idx) throws NyARException - { - if(idx>=marker_num){ - throw new NyARException(); - } - return marker_info2_array[idx]; + this.width =i_width; + this.height=i_height; + +// this.marker_holder=new NyARMarker[i_squre_max]; +// this.marker_info2_array=new NyARMarker[i_squre_max]; +// //先にマーカーホルダにオブジェクトを作っておく +// for(int i=0;i543] + * @param o_marker + * @param limage + * @param label_ref + * @param label + * @param clip + * @throws NyARException */ - private static boolean get_vertex( int[] x_coord, int[] y_coord, int st, int ed,double thresh, int vertex[],IntValue vnum) + private final void arGetContour(NyARMarker o_marker,int[][] limage, int[] label_ref,int i_labelnum, NyARLabel i_label) throws NyARException { - double d, dmax; - double a, b, c; - int i, v1=0; - - a = y_coord[ed] - y_coord[st]; - b = x_coord[st] - x_coord[ed]; - c = x_coord[ed]*y_coord[st] - y_coord[ed]*x_coord[st]; - dmax = 0; - for(i=st+1;i dmax ) { - dmax = d*d; - v1 = i; - } - } - if( dmax/(a*a+b*b) > thresh ) { - if(!get_vertex(x_coord, y_coord, st, v1, thresh, vertex, vnum)){ - return false; - } - if( vnum.get() > 5 ){ - return false; - } - vertex[vnum.get()] = v1;//vertex[(*vnum)] = v1; - vnum.inc();//(*vnum)++; - - if(!get_vertex(x_coord, y_coord, v1, ed, thresh, vertex, vnum)){ - return false; - } - } - return true; - } - /** - * static int arDetectMarker2_check_square( int area, ARMarkerInfo2 *marker_info2, double factor ) - * 関数の代替関数 - * @param area - * @param i_marker_info2 - * @param factor - * @return - */ - private static boolean check_square( int area, NyARMarker i_marker_info2, double factor ) - { - int sx, sy; - int dmax, d, v1; - int[] vertex=new int[10];//int vertex[10] - int[] wv1=new int[10],wv2=new int[10];//int wv1[10],wv2[10]; - int v2;// int wvnum1,wvnum2,v2; - double thresh; - int i; - IntValue wvnum1=new IntValue(),wvnum2=new IntValue(); - + final int[] xcoord=wk_arGetContour_xcoord; + final int[] ycoord=wk_arGetContour_ycoord; + final int[] xdir=wk_arGetContour_xdir; //static int xdir[8] = { 0, 1, 1, 1, 0,-1,-1,-1}; + final int[] ydir=wk_arGetContour_ydir;//static int ydir[8] = {-1,-1, 0, 1, 1, 1, 0,-1}; + //ShortPointer p1;//ARInt16 *p1; + int coord_num; + int sx=0, sy=0, dir; + int dmax, d, v1=0; + int i, j,w; - dmax = 0; - v1 = 0; - sx = i_marker_info2.x_coord[0];//sx = marker_info2->x_coord[0]; - sy = i_marker_info2.y_coord[0];//sy = marker_info2->y_coord[0]; - for(i=1;icoord_num-1;i++) { - d = (i_marker_info2.x_coord[i]-sx)*(i_marker_info2.x_coord[i]-sx)+ (i_marker_info2.y_coord[i]-sy)*(i_marker_info2.y_coord[i]-sy); - if( d > dmax ) { - dmax = d; - v1 = i; - } + int[] limage_j; + j = i_label.clip2; + limage_j=limage[j]; + final int clip1=i_label.clip1; + //p1=ShortPointer.wrap(limage,j*xsize+clip.get());//p1 = &(limage[j*xsize+clip[0]]); + for( i = i_label.clip0; i <= clip1; i++){//for( i = clip[0]; i <= clip[1]; i++, p1++ ) { + w=limage_j[i]; + if(w > 0 && label_ref[w-1] == i_labelnum ) {//if( *p1 > 0 && label_ref[(*p1)-1] == label ) { + sx = i; + sy = j; + break; } + } + if(i>clip1){//if( i > clip[1] ) { + System.out.println("??? 1");//printf(); + throw new NyARException();//return(-1); + } - thresh = (area/0.75) * 0.01 * factor; -// vnum = 1; - vertex[0] = 0; - wvnum1.set(0);// wvnum1 = 0; - wvnum2.set(0);// wvnum2 = 0; +// //マーカーホルダが既に確保済みかを調べる +// if(marker_holder[i_holder_num]==null){ +// //確保していなければ確保 +// marker_holder[i_holder_num]=new NyARMarker(); +// } - if(!get_vertex(i_marker_info2.x_coord, i_marker_info2.y_coord, 0, v1,thresh, wv1, wvnum1)){ //if( get_vertex(marker_info2->x_coord, marker_info2->y_coord, 0, v1,thresh, wv1, &wvnum1) < 0 ) { - return false; + + coord_num=1;//marker_info2->coord_num = 1; + xcoord[0]=sx;//marker_info2->x_coord[0] = sx; + ycoord[0]=sy;//marker_info2->y_coord[0] = sy; + dir = 5; + + int r,c; + + for(;;){ + r=ycoord[coord_num-1]; + c=xcoord[coord_num-1]; + //p1 = &(limage[marker_info2->y_coord[marker_info2->coord_num-1] * xsize+ marker_info2->x_coord[marker_info2->coord_num-1]]); + dir = (dir+5)%8; + for(i=0;i<8;i++) { + if(limage[r+ydir[dir]][c+xdir[dir]]>0){//if( p1[ydir[dir]*xsize+xdir[dir]] > 0 ){ + break; + } + dir = (dir+1)%8; } - if(!get_vertex(i_marker_info2.x_coord, i_marker_info2.y_coord,v1, i_marker_info2.coord_num-1, thresh, wv2, wvnum2)) {//if(get_vertex(marker_info2->x_coord, marker_info2->y_coord,v1, marker_info2->coord_num-1, thresh, wv2, &wvnum2) < 0 ) { - return false; + if( i == 8 ){ + System.out.println("??? 2");//printf("??? 2\n"); + throw new NyARException();//return(-1); } - - if( wvnum1.get() == 1 && wvnum2.get() == 1 ) {//if( wvnum1 == 1 && wvnum2 == 1 ) { - vertex[1] = wv1[0]; - vertex[2] = v1; - vertex[3] = wv2[0]; - }else if( wvnum1.get() > 1 && wvnum2.get() == 0 ) {//}else if( wvnum1 > 1 && wvnum2 == 0 ) { - v2 = v1 / 2; - wvnum1.set(0);wvnum2.set(0);//wvnum1 = wvnum2 = 0; - if(!get_vertex(i_marker_info2.x_coord, i_marker_info2.y_coord,0, v2, thresh, wv1, wvnum1)) { - return false; - } - if(!get_vertex(i_marker_info2.x_coord, i_marker_info2.y_coord,v2, v1, thresh, wv2, wvnum2)) { - return false; - } - if( wvnum1.get() == 1 && wvnum2.get() == 1 ) { - vertex[1] = wv1[0]; - vertex[2] = wv2[0]; - vertex[3] = v1; - }else{ - return false; - } - }else if( wvnum1.get() == 0 && wvnum2.get() > 1 ) { - v2 = (v1 + i_marker_info2.coord_num-1) / 2; - - wvnum1.set(0);wvnum2.set(0);//wvnum1 = wvnum2 = 0; - if(!get_vertex(i_marker_info2.x_coord, i_marker_info2.y_coord,v1, v2, thresh, wv1,wvnum1)) { - return false; - } - if(!get_vertex(i_marker_info2.x_coord, i_marker_info2.y_coord,v2, i_marker_info2.coord_num-1, thresh, wv2, wvnum2)) { - return false; - } - if( wvnum1.get() == 1 && wvnum2.get() == 1 ) { - vertex[1] = v1; - vertex[2] = wv1[0]; - vertex[3] = wv2[0]; - } - else { - return false; - } + xcoord[coord_num]= xcoord[coord_num-1] + xdir[dir];//marker_info2->x_coord[marker_info2->coord_num]= marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir]; + ycoord[coord_num]= ycoord[coord_num-1] + ydir[dir];//marker_info2->y_coord[marker_info2->coord_num]= marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir]; + if(xcoord[coord_num] == sx && ycoord[coord_num] == sy ){ + break; } - else { - return false; + coord_num++; + if(coord_num == xcoord.length-1){//if( marker_info2.coord_num == Config.AR_CHAIN_MAX-1 ){ + System.out.println("??? 3");//printf("??? 3\n"); + throw new NyARException();//return(-1); } + } - i_marker_info2.vertex[0] = vertex[0]; - i_marker_info2.vertex[1] = vertex[1]; - i_marker_info2.vertex[2] = vertex[2]; - i_marker_info2.vertex[3] = vertex[3]; - i_marker_info2.vertex[4] = i_marker_info2.coord_num-1; - - return true; + dmax = 0; + for(i=1;icoord_num;i++) { + d = (xcoord[i]-sx)*(xcoord[i]-sx)+ (ycoord[i]-sy)*(ycoord[i]-sy);// d = (marker_info2->x_coord[i]-sx)*(marker_info2->x_coord[i]-sx)+ (marker_info2->y_coord[i]-sy)*(marker_info2->y_coord[i]-sy); + if( d > dmax ) { + dmax = d; + v1 = i; + } } + //NyARMarkerへcoord情報をセット + //coordの並び替えと保存はNyARMarkerへ移動 + o_marker.setCoordXY(v1,coord_num,xcoord,ycoord); + +// int[] wx=new int[v1];//new int[Config.AR_CHAIN_MAX]; +// int[] wy=new int[v1]; //new int[Config.AR_CHAIN_MAX]; +// for(i=0;ix_coord[i]; +// wy[i] = marker_ref.y_coord[i];//wy[i] = marker_info2->y_coord[i]; +// } +// for(i=0;icoord_num;i++) { +// marker_ref.x_coord[i] = marker_ref.x_coord[i+v1];//marker_info2->x_coord[i-v1] = marker_info2->x_coord[i]; +// marker_ref.y_coord[i] = marker_ref.y_coord[i+v1];//marker_info2->y_coord[i-v1] = marker_info2->y_coord[i]; +// } +// for(i=0;ix_coord[i-v1+marker_info2->coord_num] = wx[i]; +// marker_ref.y_coord[i-v1+marker_ref.coord_num] = wy[i];//marker_info2->y_coord[i-v1+marker_info2->coord_num] = wy[i]; +// } +// marker_ref.x_coord[marker_ref.coord_num] = marker_ref.x_coord[0];//marker_info2->x_coord[marker_info2->coord_num] = marker_info2->x_coord[0]; +// marker_ref.y_coord[marker_ref.coord_num] = marker_ref.y_coord[0];//marker_info2->y_coord[marker_info2->coord_num] = marker_info2->y_coord[0]; +// marker_ref.coord_num++;//marker_info2->coord_num++; + return; + } + /** - * int arGetContour( ARInt16 *limage, int *label_ref,int label, int clip[4], ARMarkerInfo2 *marker_info2 ) + * ARMarkerInfo2 *arDetectMarker2( ARInt16 *limage, int label_num, int *label_ref,int *warea, double *wpos, int *wclip,int area_max, int area_min, double factor, int *marker_num ) * 関数の代替品 - * detectMarker関数から使う関数です。marker_holder[i_holder_num]にオブジェクトが無ければまず新規に作成し、もし - * 既に存在すればそこにマーカー情報を上書きして記録します。 - * @param limage - * @param label_ref - * @param label - * @param clip - * @return - * 検出したマーカーからマーカーオブジェクトを生成して返す。 + * ラベリング情報からマーカー一覧を作成してo_marker_listを更新します。 + * 関数はo_marker_listに重なりを除外したマーカーリストを作成します。 + * + * @param i_labeling + * ラベリング済みの情報を持つラベリングオブジェクト + * @param i_factor + * 何かの閾値？ + * @param o_marker_list + * 抽出したマーカーを格納するリスト * @throws NyARException */ - private NyARMarker arGetContour(int i_holder_num,short[][] limage, int[] label_ref,int label, int[] clip) throws NyARException + public final void detectMarker(NyARLabeling i_labeling,double i_factor,NyARMarkerList o_marker_list) throws NyARException { - final int[] xdir={0, 1, 1, 1, 0,-1,-1,-1}; //static int xdir[8] = { 0, 1, 1, 1, 0,-1,-1,-1}; - final int[] ydir={-1,-1, 0, 1, 1, 1, 0,-1};//static int ydir[8] = {-1,-1, 0, 1, 1, 1, 0,-1}; - //ShortPointer p1;//ARInt16 *p1; - int sx=0, sy=0, dir; - int dmax, d, v1=0; - int i, j; - - j = clip[2]; - //p1=ShortPointer.wrap(limage,j*xsize+clip.get());//p1 = &(limage[j*xsize+clip[0]]); - for( i = clip[0]; i <= clip[1]; i++){//for( i = clip[0]; i <= clip[1]; i++, p1++ ) { - if(limage[j][i] > 0 && label_ref[(limage[j][i])-1] == label ) {//if( *p1 > 0 && label_ref[(*p1)-1] == label ) { - sx = i; sy = j; - break; - } - } - if(i> clip[1]){//if( i > clip[1] ) { - System.out.println("??? 1");//printf(); - throw new NyARException();//return(-1); - } - - //マーカーホルダが既に確保済みかを調べる - if(marker_holder[i_holder_num]==null){ - //確保していなければ確保 - marker_holder[i_holder_num]=new NyARMarker(); - } - NyARMarker marker_ref=marker_holder[i_holder_num]; + int i; + int xsize, ysize; + NyARLabel[] labels=i_labeling.getLabel(); +// int[] warea =i_labeling.getArea(); + int label_num =i_labeling.getLabelNum(); +// int[][] wclip =i_labeling.getClip(); +// double[] wpos =i_labeling.getPos(); + int[][] limage=i_labeling.getLabelImg(); + int[] label_ref =i_labeling.getLabelRef(); - - marker_ref.coord_num=1;//marker_info2->coord_num = 1; - marker_ref.x_coord[0]=sx;//marker_info2->x_coord[0] = sx; - marker_ref.y_coord[0]=sy;//marker_info2->y_coord[0] = sy; - dir = 5; - - for(;;){ - int r=marker_ref.y_coord[marker_ref.coord_num-1]; - int c=marker_ref.x_coord[marker_ref.coord_num-1]; - //p1 = &(limage[marker_info2->y_coord[marker_info2->coord_num-1] * xsize+ marker_info2->x_coord[marker_info2->coord_num-1]]); - dir = (dir+5)%8; - for(i=0;i<8;i++) { - if(limage[r+ydir[dir]][c+xdir[dir]]>0){//if( p1[ydir[dir]*xsize+xdir[dir]] > 0 ){ - break; - } - dir = (dir+1)%8; - } - if( i == 8 ){ - System.out.println("??? 2");//printf("??? 2\n"); - throw new NyARException();//return(-1); - } - marker_ref.x_coord[marker_ref.coord_num]= marker_ref.x_coord[marker_ref.coord_num-1] + xdir[dir];//marker_info2->x_coord[marker_info2->coord_num]= marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir]; - marker_ref.y_coord[marker_ref.coord_num]= marker_ref.y_coord[marker_ref.coord_num-1] + ydir[dir];//marker_info2->y_coord[marker_info2->coord_num]= marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir]; - if( marker_ref.x_coord[marker_ref.coord_num] == sx && marker_ref.y_coord[marker_ref.coord_num] == sy ){ - break; - } - marker_ref.coord_num++; - if( marker_ref.coord_num == marker_ref.x_coord.length-1){//if( marker_info2.coord_num == Config.AR_CHAIN_MAX-1 ){ - System.out.println("??? 3");//printf("??? 3\n"); - throw new NyARException();//return(-1); - } - } - - dmax = 0; - for(i=1;icoord_num;i++) { - d = (marker_ref.x_coord[i]-sx)*(marker_ref.x_coord[i]-sx)+ (marker_ref.y_coord[i]-sy)*(marker_ref.y_coord[i]-sy);// d = (marker_info2->x_coord[i]-sx)*(marker_info2->x_coord[i]-sx)+ (marker_info2->y_coord[i]-sy)*(marker_info2->y_coord[i]-sy); - if( d > dmax ) { - dmax = d; - v1 = i; - } - } + //マーカーホルダをリセット + o_marker_list.reset(); +// marker_num=0; + xsize =width; + ysize =height; +// マーカーをmarker_holderに蓄積する。 + NyARMarker current_marker=o_marker_list.getCurrentHolder(); + NyARLabel label_pt; + for(i=0; i AR_AREA_MAX ){ + continue; + } + if( label_pt.clip0 == 1 || label_pt.clip1 == xsize-2 ){//if( wclip[i*4+0] == 1 || wclip[i*4+1] == xsize-2 ){ + continue; + } + if( label_pt.clip2 == 1 || label_pt.clip3 == ysize-2 ){//if( wclip[i*4+2] == 1 || wclip[i*4+3] == ysize-2 ){ + continue; + } + //ret = arGetContour( limage, label_ref, i+1,&(wclip[i*4]), &(marker_info2[marker_num2])); + arGetContour(current_marker,limage, label_ref, i+1,label_pt); - int[] wx=new int[v1];//new int[Config.AR_CHAIN_MAX]; - int[] wy=new int[v1]; //new int[Config.AR_CHAIN_MAX]; - for(i=0;ix_coord[i]; - wy[i] = marker_ref.y_coord[i];//wy[i] = marker_info2->y_coord[i]; - } - for(i=v1;icoord_num;i++) { - marker_ref.x_coord[i-v1] = marker_ref.x_coord[i];//marker_info2->x_coord[i-v1] = marker_info2->x_coord[i]; - marker_ref.y_coord[i-v1] = marker_ref.y_coord[i];//marker_info2->y_coord[i-v1] = marker_info2->y_coord[i]; - } - for(i=0;ix_coord[i-v1+marker_info2->coord_num] = wx[i]; - marker_ref.y_coord[i-v1+marker_ref.coord_num] = wy[i];//marker_info2->y_coord[i-v1+marker_info2->coord_num] = wy[i]; - } - marker_ref.x_coord[marker_ref.coord_num] = marker_ref.x_coord[0];//marker_info2->x_coord[marker_info2->coord_num] = marker_info2->x_coord[0]; - marker_ref.y_coord[marker_ref.coord_num] = marker_ref.y_coord[0];//marker_info2->y_coord[marker_info2->coord_num] = marker_info2->y_coord[0]; - marker_ref.coord_num++;//marker_info2->coord_num++; - - return marker_ref; - } - /** - * ARMarkerInfo2 *arDetectMarker2( ARInt16 *limage, int label_num, int *label_ref,int *warea, double *wpos, int *wclip,int area_max, int area_min, double factor, int *marker_num ) - * 関数の代替品 - * ラベリング情報からマーカー一覧を作成して保持します。 - * この関数を実行すると、前回のdetectMarker関数で計算した保持値は破壊されます。 - * @param i_labeling - * ラベリング済みの情報を持つラベリングオブジェクト - * @param factor - * 何かの閾値？ - * @return - * @throws NyARException - */ -// public void detectMarker(short[][] limage,int label_num,int[] label_ref,int[] warea,double[] wpos,int[] wclip,int area_max, int area_min, double factor) throws JartkException - public void detectMarker(NyARLabeling i_labeling,double factor) throws NyARException - { - int xsize, ysize; - int marker_num2; - double d; - int[] warea =i_labeling.getArea(); - int label_num =i_labeling.getLabelNum(); - int[][] wclip =i_labeling.getClip(); - double[] wpos =i_labeling.getPos(); - short[][] limage =i_labeling.getLabelImg(); - int[] label_ref =i_labeling.getLabelRef(); - - marker_num=0; - xsize =width; - ysize =height; - - marker_num2 = 0; - for(int i=0; i area_max ){ - continue; - } - if( wclip[i][0] == 1 || wclip[i][1] == xsize-2 ){//if( wclip[i*4+0] == 1 || wclip[i*4+1] == xsize-2 ){ - continue; - } - if( wclip[i][2] == 1 || wclip[i][3] == ysize-2 ){//if( wclip[i*4+2] == 1 || wclip[i*4+3] == ysize-2 ){ - continue; - } - //ret = arGetContour( limage, label_ref, i+1,&(wclip[i*4]), &(marker_info2[marker_num2])); - arGetContour(marker_num2,limage, label_ref, i+1,wclip[i]); - - boolean ret = check_square( warea[i], marker_holder[marker_num2], factor );//ret = check_square( warea[i], &(marker_info2[marker_num2]), factor ); - if(!ret){ - //後半で整理するからここはいらない。// marker_holder[marker_num2]=null; - continue; - } - marker_holder[marker_num2].area = warea[i]; - marker_holder[marker_num2].pos[0] = wpos[i*2+0]; - marker_holder[marker_num2].pos[1] = wpos[i*2+1]; - marker_num2++; - //マーカーリストが上限に達した - if( marker_num2 == marker_holder.length){ - break; - } - } - for(int i=0; i < marker_num2; i++ ) { - for(int j=i+1; j < marker_num2; j++ ) { - d = (marker_holder[i].pos[0] - marker_holder[j].pos[0])* - (marker_holder[i].pos[0] - marker_holder[j].pos[0])+ - (marker_holder[i].pos[1] - marker_holder[j].pos[1])* - (marker_holder[i].pos[1] - marker_holder[j].pos[1]); - if(marker_holder[i].area >marker_holder[j].area ) { - if( d marker_holder[j].area ) { +// if( d =marker_info.length){ - break; - } - } - number_of_square=j; - } - /** - * arGetLine(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2]) - * arGetLine2(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2], double *dist_factor) - * の２関数の合成品です。 - * @param x_coord - * @param y_coord - * @param coord_num - * @param vertex - * @param line - * @param v - * @return - * @throws NyARException - */ - private boolean getLine(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[][], double v[][]) throws NyARException - { - NyARMat input,evec; - NyARVec ev,mean; - double w1; - int st, ed, n; - int i, j; - DoubleValue dv1=new DoubleValue(); - DoubleValue dv2=new DoubleValue(); - - ev = new NyARVec(2); - mean = new NyARVec(2); - evec = new NyARMat(2,2); - double[] mean_array=mean.getArray(); - double[][] evec_array=evec.getArray(); - for( i = 0; i < 4; i++ ) { - w1 = (double)(vertex[i+1]-vertex[i]+1) * 0.05 + 0.5; - st = (int)(vertex[i] + w1); - ed = (int)(vertex[i+1] - w1); - n = ed - st + 1; - if(n<2){//nが2以下でmatrix.PCAを計算することはできないので、エラーにしておく。 - //System.err.println("NyARDetectSquare::getLine 稀に出るエラーです。このエラーが出ても例外が起こらなければ平気だと思いますが、出たらnyatlaまで連絡してください。"); - return false;//throw new NyARException(); - } - input = new NyARMat( n, 2 ); - double [][] in_array=input.getArray(); - for( j = 0; j < n; j++ ) { - param.observ2Ideal(x_coord[st+j], y_coord[st+j],dv1,dv2);//arParamObserv2Ideal( dist_factor, x_coord[st+j], y_coord[st+j],&(input->m[j*2+0]), &(input->m[j*2+1]) ); - in_array[j][0]=dv1.value; - in_array[j][1]=dv2.value; - } - NyARMat.matrixPCA(input, evec, ev, mean); - - line[i][0] = evec_array[0][1];//line[i][0] = evec->m[1]; - line[i][1] = -evec_array[0][0];//line[i][1] = -evec->m[0]; - line[i][2] = -(line[i][0]*mean_array[0] + line[i][1]*mean_array[1]);//line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]); - } - - for( i = 0; i < 4; i++ ) { - w1 = line[(i+3)%4][0] * line[i][1] - line[i][0] * line[(i+3)%4][1]; - if( w1 == 0.0 ){ - return false; - } - v[i][0] = ( line[(i+3)%4][1] * line[i][2]- line[i][1] * line[(i+3)%4][2] ) / w1; - v[i][1] = ( line[i][0] * line[(i+3)%4][2]- line[(i+3)%4][0] * line[i][2] ) / w1; - } - return true; +// NyARSquare square; +// int j=0; +// for (int i = 0; i =marker_info.length){ +// break; +// } +// } +// number_of_square=j; } } diff --git a/src/jp/nyatla/nyartoolkit/core/NyARLabeling.java b/src/jp/nyatla/nyartoolkit/core/NyARLabeling.java index 7fb86d9..a1bde60 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARLabeling.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARLabeling.java @@ -35,20 +35,186 @@ import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.raster.*; +interface NyARLabeling{ + /** + * 検出したラベルの数を返す + * @return + */ + public int getLabelNum(); + /** + * + * @return + * @throws NyARException + */ + public int[] getLabelRef() throws NyARException; + /** + * 検出したラベル配列 + * @return + * @throws NyARException + */ + public NyARLabel[] getLabel() throws NyARException; + /** + * ラベリング済みイメージを返す + * @return + * @throws NyARException + */ + public int[][] getLabelImg() throws NyARException; + /** + * static ARInt16 *labeling2( ARUint8 *image, int thresh,int *label_num, int **area, double **pos, int **clip,int **label_ref, int LorR ) + * 関数の代替品 + * ラスタimageをラベリングして、結果を保存します。 + * Optimize:STEP[1514->1493] + * @param image + * @param thresh + * @throws NyARException + */ + public void labeling(NyARRaster image,int thresh) throws NyARException; +} + + + + +/** + * NyARLabeling_O2のworkとwork2を可変長にするためのクラス + * + * + */ +class NyARWorkHolder +{ + private final static int ARRAY_APPEND_STEP=256; + public final int[] work; + public final int[][] work2; + private int allocate_size; + /** + * 最大i_holder_size個の動的割り当てバッファを準備する。 + * @param i_holder_size + */ + public NyARWorkHolder(int i_holder_size) + { + //ポインタだけははじめに確保しておく + this.work=new int[i_holder_size]; + this.work2=new int[i_holder_size][]; + this.allocate_size=0; + } + /** + * i_indexで指定した番号までのバッファを準備する。 + * @param i_index + */ + public final void reserv(int i_index) throws NyARException + { + //アロケート済みなら即リターン + if(this.allocate_size>i_index){ + return; + } + //要求されたインデクスは範囲外 + if(i_index>=this.work.length){ + throw new NyARException(); + } + //追加アロケート範囲を計算 + int range=i_index+ARRAY_APPEND_STEP; + if(range>=this.work.length){ + range=this.work.length; + } + //アロケート + for(int i=this.allocate_size;ii_index){ + return; + } + //要求されたインデクスは範囲外 + if(i_index>=this.labels.length){ + throw new NyARException(); + } + //追加アロケート範囲を計算 + int range=i_index+ARRAY_APPEND_STEP; + if(range>=this.labels.length){ + range=this.labels.length; + } + //アロケート + for(int i=this.allocate_size;i1493] * @param image * @param thresh * @throws NyARException */ public void labeling(NyARRaster image,int thresh) throws NyARException { - int wk_max; /* work */ - int m,n; /* work */ - int lxsize, lysize; - int[] warea;//int *warea; - int[][] wclip;//int *wclip; - double[] wpos;//double *wpos; - int thresht3 = thresh * 3; - - //ラベル数を0に初期化 - label_num=0; - - warea=area;//warea = &wareaL[0]; - wclip=clip;//wclip = &wclipL[0]; - wpos=pos;//wpos = &wposL[0]; - - lxsize=image.getWidth();//lxsize = arUtil_c.arImXsize; - lysize=image.getHeight();//lysize = arUtil_c.arImYsize; - - for(int i = 0; i < lxsize; i++){ - label_img[0][i]=0; - label_img[lysize-1][i]=0; - } - for(int i = 0; i < lysize; i++) { - label_img[i][0]=0; - label_img[i][lxsize-1]=0; - } - int nya_pnt_start_x_start,nya_pnt_start_y_start; - int nya_poff_step;//スキャンステップ - - wk_max = 0; - nya_pnt_start_y_start=1; - nya_pnt_start_x_start=1; - nya_poff_step=1;//スキャンステップ - int nya_pnt_start_y=nya_pnt_start_y_start; - for (int j = 1; j < lysize - 1; j++, nya_pnt_start_y++) {//for (int j = 1; j < lysize - 1; j++, pnt += poff*2, pnt2 += 2) { - int nya_pnt_start_x=nya_pnt_start_x_start; - int p1=j-1; - int p2=j; - for(int i = 1; i < lxsize-1; i++, nya_pnt_start_x+=nya_poff_step) {//for(int i = 1; i < lxsize-1; i++, pnt+=poff, pnt2++) { + int wk_max; /* work */ + int m,n; /* work */ + int lxsize, lysize; + int thresht3 = thresh * 3; + int i,j,k; + + //ラベル数を0に初期化 + this.label_num=0; +// int[][] work2=this.wk_labeling_work2; +// int[] work=this.gwork; + + int[][] label_img=this.glabel_img; + + lxsize=image.getWidth();//lxsize = arUtil_c.arImXsize; + lysize=image.getHeight();//lysize = arUtil_c.arImYsize; + + int[] label_img_pt0,label_img_pt1; + + //label_img_pt0,label_img_pt1 + label_img_pt0=label_img[0]; + label_img_pt1=label_img[lysize-1]; + for(i = 0; i < lxsize; i++){ + label_img_pt0[i]=0; + label_img_pt1[i]=0; + } + // + for(i = 0; i < lysize; i++) { + label_img[i][0]=0; + label_img[i][lxsize-1]=0; + } + + int[] work2_pt; + wk_max = 0; + + int label_pixel; + + int[] work=this.work_holder.work; + int[][] work2=this.work_holder.work2; + //1ライン分のメモリを予約 + int[] line_bufferr=reservWorkLineBuffer(lxsize); + + for(j = 1; j < lysize - 1; j++) {//for (int j = 1; j < lysize - 1; j++, pnt += poff*2, pnt2 += 2) { + label_img_pt0=label_img[j]; + label_img_pt1=label_img[j-1]; + image.getPixelTotalRowLine(j,line_bufferr); + + for(i = 1; i < lxsize-1; i++) {//for(int i = 1; i < lxsize-1; i++, pnt+=poff, pnt2++) { //RGBの合計値が閾値より大きいかな？ - if(image.getPixelTotal(nya_pnt_start_x,nya_pnt_start_y)<=thresht3){ + if(line_bufferr[i]<=thresht3){ //pnt1 = ShortPointer.wrap(pnt2, -lxsize);//pnt1 = &(pnt2[-lxsize]); - if(label_img[p1][i]>0){//if( *pnt1 > 0 ) { - label_img[p2][i]=label_img[p1][i];//*pnt2 = *pnt1; - - - int p2_index=(label_img[p2][i]-1)*7; - work2[p2_index+0]++;//work2[((*pnt2)-1)*7+0] ++; - work2[p2_index+1]+=i;//work2[((*pnt2)-1)*7+1] += i; - work2[p2_index+2]+=j;//work2[((*pnt2)-1)*7+2] += j; - work2[p2_index+6]=j;//work2[((*pnt2)-1)*7+6] = j; - }else if(label_img[p1][i+1]> 0 ) {//}else if( *(pnt1+1) > 0 ) { - if(label_img[p1][i-1] > 0 ) {//if( *(pnt1-1) > 0 ) { - m = work[label_img[p1][i+1]-1];//m = work[*(pnt1+1)-1]; - n = work[label_img[p1][i-1]-1];//n = work[*(pnt1-1)-1]; + if(label_img_pt1[i]>0){//if( *pnt1 > 0 ) { + label_pixel=label_img_pt1[i];//*pnt2 = *pnt1; + + + work2_pt=work2[label_pixel-1]; + work2_pt[0]++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1]+=i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2]+=j;//work2[((*pnt2)-1)*7+2] += j; + work2_pt[6]=j;//work2[((*pnt2)-1)*7+6] = j; + }else if(label_img_pt1[i+1]> 0 ) {//}else if( *(pnt1+1) > 0 ) { + if(label_img_pt1[i-1] > 0 ) {//if( *(pnt1-1) > 0 ) { + m = work[label_img_pt1[i+1]-1];//m = work[*(pnt1+1)-1]; + n = work[label_img_pt1[i-1]-1];//n = work[*(pnt1-1)-1]; if( m > n ){ - //JartkException.trap("未チェックのパス"); - label_img[p2][i]=(short)n;//*pnt2 = n; + label_pixel=n;//*pnt2 = n; //wk=IntPointer.wrap(work, 0);//wk = &(work[0]); - for(int k = 0; k < wk_max; k++) { - //JartkException.trap("未チェックのパス"); + for(k = 0; k < wk_max; k++) { if(work[k] == m ){//if( *wk == m ) - //JartkException.trap("未チェックのパス"); work[k]=n;//*wk = n; } } }else if( m < n ) { - //JartkException.trap("未チェックのパス"); - label_img[p2][i]=(short)m;//*pnt2 = m; + label_pixel=m;//*pnt2 = m; //wk=IntPointer.wrap(work,0);//wk = &(work[0]); - for(int k = 0; k < wk_max; k++){ - //JartkException.trap("未チェックのパス"); + for(k = 0; k < wk_max; k++){ if(work[k]==n){//if( *wk == n ){ - //JartkException.trap("未チェックのパス"); work[k]=m;//*wk = m; } } }else{ - label_img[p2][i]=(short)m;//*pnt2 = m; + label_pixel=m;//*pnt2 = m; } - - int p2_index=(label_img[p2][i]-1)*7; - work2[p2_index+0] ++; - work2[p2_index+1] += i; - work2[p2_index+2] += j; - work2[p2_index+6] = j; - }else if( (label_img[p2][i-1]) > 0 ) {//}else if( *(pnt2-1) > 0 ) { - m = work[(label_img[p1][i+1])-1];//m = work[*(pnt1+1)-1]; - n = work[(label_img[p2][i-1])-1];//n = work[*(pnt2-1)-1]; + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++; + work2_pt[1] += i; + work2_pt[2] += j; + work2_pt[6] = j; + }else if( (label_img_pt0[i-1]) > 0 ) {//}else if( *(pnt2-1) > 0 ) { + m = work[(label_img_pt1[i+1])-1];//m = work[*(pnt1+1)-1]; + n = work[label_img_pt0[i-1]-1];//n = work[*(pnt2-1)-1]; if( m > n ) { - label_img[p2][i]=(short)n;//*pnt2 = n; - for(int k = 0; k < wk_max; k++) { + label_pixel=n;//*pnt2 = n; + for(k = 0; k < wk_max; k++) { if(work[k]==m){//if( *wk == m ){ work[k]=n;//*wk = n; } } }else if( m < n ) { - label_img[p2][i]=(short)m;//*pnt2 = m; - for(int k = 0; k < wk_max; k++) { + label_pixel=m;//*pnt2 = m; + for(k = 0; k < wk_max; k++) { if(work[k]==n){//if( *wk == n ){ work[k]=m;//*wk = m; } } }else{ - label_img[p2][i]=(short)m;//*pnt2 = m; + label_pixel=m;//*pnt2 = m; } - - - int p2_index=((label_img[p2][i])-1)*7; - work2[p2_index+0] ++;//work2[((*pnt2)-1)*7+0] ++; - work2[p2_index+1] += i;//work2[((*pnt2)-1)*7+1] += i; - work2[p2_index+2] += j;//work2[((*pnt2)-1)*7+2] += j; + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; }else{ - label_img[p2][i]=label_img[p1][i+1];//*pnt2 = *(pnt1+1); - - int p2_index=((label_img[p2][i])-1)*7; - work2[p2_index+0] ++;//work2[((*pnt2)-1)*7+0] ++; - work2[p2_index+1] += i;//work2[((*pnt2)-1)*7+1] += i; - work2[p2_index+2] += j;//work2[((*pnt2)-1)*7+2] += j; - if( work2[p2_index+3] > i ){//if( work2[((*pnt2)-1)*7+3] > i ){ - work2[p2_index+3] = i;// work2[((*pnt2)-1)*7+3] = i; + label_pixel=label_img_pt1[i+1];//*pnt2 = *(pnt1+1); + + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; + if( work2_pt[3] > i ){//if( work2[((*pnt2)-1)*7+3] > i ){ + work2_pt[3] = i;// work2[((*pnt2)-1)*7+3] = i; } - work2[p2_index+6] = j;//work2[((*pnt2)-1)*7+6] = j; + work2_pt[6] = j;//work2[((*pnt2)-1)*7+6] = j; } - }else if( (label_img[p1][i-1]) > 0 ) {//}else if( *(pnt1-1) > 0 ) { - label_img[p2][i]=label_img[p1][i-1];//*pnt2 = *(pnt1-1); - - int p2_index=((label_img[p2][i])-1)*7; - work2[p2_index+0] ++;//work2[((*pnt2)-1)*7+0] ++; - work2[p2_index+1] += i;//work2[((*pnt2)-1)*7+1] += i; - work2[p2_index+2] += j;//work2[((*pnt2)-1)*7+2] += j; - if( work2[p2_index+4] < i ){//if( work2[((*pnt2)-1)*7+4] < i ){ - work2[p2_index+4] = i;// work2[((*pnt2)-1)*7+4] = i; + }else if( (label_img_pt1[i-1]) > 0 ) {//}else if( *(pnt1-1) > 0 ) { + label_pixel=label_img_pt1[i-1];//*pnt2 = *(pnt1-1); + + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; + if( work2_pt[4] < i ){//if( work2[((*pnt2)-1)*7+4] < i ){ + work2_pt[4] = i;// work2[((*pnt2)-1)*7+4] = i; } - work2[p2_index+6] = j;//work2[((*pnt2)-1)*7+6] = j; - }else if(label_img[p2][i-1] > 0) {//}else if( *(pnt2-1) > 0) { - label_img[p2][i]=label_img[p2][i-1];//*pnt2 = *(pnt2-1); - - int p2_index=((label_img[p2][i])-1)*7; - work2[p2_index+0] ++;//work2[((*pnt2)-1)*7+0] ++; - work2[p2_index+1] += i;//work2[((*pnt2)-1)*7+1] += i; - work2[p2_index+2] += j;//work2[((*pnt2)-1)*7+2] += j; - if( work2[p2_index+4] < i ){//if( work2[((*pnt2)-1)*7+4] < i ){ - work2[p2_index+4] = i;// work2[((*pnt2)-1)*7+4] = i; + work2_pt[6] = j;//work2[((*pnt2)-1)*7+6] = j; + }else if(label_img_pt0[i-1] > 0) {//}else if( *(pnt2-1) > 0) { + label_pixel=label_img_pt0[i-1];//*pnt2 = *(pnt2-1); + + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; + if(work2_pt[4] < i ){//if( work2[((*pnt2)-1)*7+4] < i ){ + work2_pt[4] = i;// work2[((*pnt2)-1)*7+4] = i; } }else{ + //現在地までの領域を予約 + this.work_holder.reserv(wk_max); wk_max++; - if( wk_max > WORK_SIZE ) { - throw new NyARException();//return (0); - } - work[wk_max-1] = wk_max;label_img[p2][i]=(short)wk_max;//work[wk_max-1] = *pnt2 = wk_max; - work2[(wk_max-1)*7+0] = 1; - work2[(wk_max-1)*7+1] = i; - work2[(wk_max-1)*7+2] = j; - work2[(wk_max-1)*7+3] = i; - work2[(wk_max-1)*7+4] = i; - work2[(wk_max-1)*7+5] = j; - work2[(wk_max-1)*7+6] = j; + work[wk_max-1] = wk_max; + label_pixel=wk_max;//work[wk_max-1] = *pnt2 = wk_max; + work2_pt=work2[wk_max-1]; + work2_pt[0] = 1; + work2_pt[1] = i; + work2_pt[2] = j; + work2_pt[3] = i; + work2_pt[4] = i; + work2_pt[5] = j; + work2_pt[6] = j; } + label_img_pt0[i]=label_pixel; }else { - label_img[p2][i]=0;//*pnt2 = 0; + label_img_pt0[i]=0;//*pnt2 = 0; } + } } - int j = 1; - for(int i = 0; i < wk_max; i++){//for(int i = 1; i <= wk_max; i++, wk++) { + j = 1; + for(i = 0; i < wk_max; i++){//for(int i = 1; i <= wk_max; i++, wk++) { work[i]=(work[i]==i+1)? j++: work[work[i]-1];//*wk = (*wk==i)? j++: work[(*wk)-1]; } - + int wlabel_num=j - 1;//*label_num = *wlabel_num = j - 1; if(wlabel_num==0){//if( *label_num == 0 ) { @@ -344,38 +487,67 @@ public class NyARLabeling{ return; } - putZero(warea,wlabel_num);//put_zero( (ARUint8 *)warea, *label_num * sizeof(int) ); - putZero(wpos,wlabel_num*2);//put_zero( (ARUint8 *)wpos, *label_num * 2 * sizeof(double) ); - for(int i = 0; i < wlabel_num; i++) {//for(i = 0; i < *label_num; i++) { - wclip[i][0] = lxsize;//wclip[i*4+0] = lxsize; - wclip[i][1] = 0;//wclip[i*4+1] = 0; - wclip[i][2] = lysize;//wclip[i*4+2] = lysize; - wclip[i][3] = 0;//wclip[i*4+3] = 0; - } - for(int i = 0; i < wk_max; i++) { - j = work[i] - 1; - warea[j] += work2[i*7+0]; - wpos[j*2+0] += work2[i*7+1]; - wpos[j*2+1] += work2[i*7+2]; - if( wclip[j][0] > work2[i*7+3] ){ - wclip[j][0] = work2[i*7+3]; + + + //ラベルバッファを予約&初期化 + this.label_holder.init(wlabel_num, lxsize, lysize); +// +// putZero(warea,wlabel_num);//put_zero( (ARUint8 *)warea, *label_num * sizeof(int) ); +// for(i=0;i work2_pt[3] ){ + label_pt.clip0 = work2_pt[3]; } - if( wclip[j][1] < work2[i*7+4] ){ - wclip[j][1] = work2[i*7+4]; + if( label_pt.clip1 < work2_pt[4] ){ + label_pt.clip1 = work2_pt[4]; } - if( wclip[j][2] > work2[i*7+5] ){ - wclip[j][2] = work2[i*7+5]; + if(label_pt.clip2 > work2_pt[5] ){ + label_pt.clip2 = work2_pt[5]; } - if( wclip[j][3] < work2[i*7+6] ){ - wclip[j][3] = work2[i*7+6]; + if( label_pt.clip3 < work2_pt[6] ){ + label_pt.clip3 = work2_pt[6]; } + +// warea[j] += work2_pt[0]; +// wpos[j*2+0] += work2_pt[1]; +// wpos[j*2+1] += work2_pt[2]; +// if( wclip[j][0] > work2_pt[3] ){ +// wclip[j][0] = work2_pt[3]; +// } +// if( wclip[j][1] < work2_pt[4] ){ +// wclip[j][1] = work2_pt[4]; +// } +// if( wclip[j][2] > work2_pt[5] ){ +// wclip[j][2] = work2_pt[5]; +// } +// if( wclip[j][3] < work2_pt[6] ){ +// wclip[j][3] = work2_pt[6]; +// } } - for(int i = 0; i < wlabel_num; i++ ) {//for(int i = 0; i < *label_num; i++ ) { - wpos[i*2+0] /= warea[i]; - wpos[i*2+1] /= warea[i]; + for(i = 0; i < wlabel_num; i++ ) {//for(int i = 0; i < *label_num; i++ ) { + label_pt=labels[i]; + label_pt.pos_x /= label_pt.area; + label_pt.pos_y /= label_pt.area; } - + label_num=wlabel_num; return; } diff --git a/src/jp/nyatla/nyartoolkit/core/NyARMarker.java b/src/jp/nyatla/nyartoolkit/core/NyARMarker.java index 48fdaa6..7fd0989 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARMarker.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARMarker.java @@ -35,6 +35,8 @@ package jp.nyatla.nyartoolkit.core; + + /** * typedef struct { * int area; @@ -48,13 +50,228 @@ package jp.nyatla.nyartoolkit.core; */ class NyARMarker { - private static final int AR_CHAIN_MAX=10000; - int area; - double[] pos=new double[2]; + /** + * メモリブロックのサイズ(32*4=128kb) + */ + private static final int ALLOCATE_PAGE_SIZE=256; + /** + * メモリブロックの初期サイズ + */ + private static final int INITIAL_SIZE=1; + int[] x_coord=new int[INITIAL_SIZE]; + int[] y_coord=new int[INITIAL_SIZE]; int coord_num; - int[] x_coord=new int[AR_CHAIN_MAX]; - int[] y_coord=new int[AR_CHAIN_MAX]; - int[] vertex=new int[5]; + int area; + final double[] pos=new double[2]; + final int[] mkvertex=new int[5]; + /** + * coordバッファをi_chain_num以上のサイズに再アロケートする。 + * 内容の引継ぎは行われない。 + * @param i_chain_num + */ + private void reallocCoordArray(int i_chain_num) + { + if(x_coord.lengthx_coord[i]; +// wy[i] = marker_ref.y_coord[i];//wy[i] = marker_info2->y_coord[i]; +// } +// for(i=0;icoord_num;i++) { +// marker_ref.x_coord[i] = marker_ref.x_coord[i+v1];//marker_info2->x_coord[i-v1] = marker_info2->x_coord[i]; +// marker_ref.y_coord[i] = marker_ref.y_coord[i+v1];//marker_info2->y_coord[i-v1] = marker_info2->y_coord[i]; +// } +// for(i=0;ix_coord[i-v1+marker_info2->coord_num] = wx[i]; +// marker_ref.y_coord[i-v1+marker_ref.coord_num] = wy[i];//marker_info2->y_coord[i-v1+marker_info2->coord_num] = wy[i]; +// } +// marker_ref.x_coord[marker_ref.coord_num] = marker_ref.x_coord[0];//marker_info2->x_coord[marker_info2->coord_num] = marker_info2->x_coord[0]; +// marker_ref.y_coord[marker_ref.coord_num] = marker_ref.y_coord[0];//marker_info2->y_coord[marker_info2->coord_num] = marker_info2->y_coord[0]; +// marker_ref.coord_num++;//marker_info2->coord_num++; + } + private final NyARVertexCounter wk_checkSquare_wv1=new NyARVertexCounter(); + private final NyARVertexCounter wk_checkSquare_wv2=new NyARVertexCounter(); + /** + * static int arDetectMarker2_check_square( int area, ARMarkerInfo2 *marker_info2, double factor ) + * 関数の代替関数 + * OPTIMIZED STEP [450->397] + * @param i_area + * @param i_factor + * @return + */ + public boolean checkSquare(int i_area,double i_factor,double i_pos_x,double i_pos_y) + { + final int[] l_vertex=mkvertex; + final int[] l_x_coord=x_coord; + final int[] l_y_coord=y_coord; + final NyARVertexCounter wv1=wk_checkSquare_wv1; + final NyARVertexCounter wv2=wk_checkSquare_wv2; + int sx, sy; + int dmax, d, v1; + + int v2;// int wvnum1,wvnum2,v2; + double thresh; + int i; + + dmax = 0; + v1 = 0; + sx = l_x_coord[0];//sx = marker_info2->x_coord[0]; + sy = l_y_coord[0];//sy = marker_info2->y_coord[0]; + for(i=1;icoord_num-1;i++) { + d = (l_x_coord[i]-sx)*(l_x_coord[i]-sx)+ (l_y_coord[i]-sy)*(l_y_coord[i]-sy); + if( d > dmax ) { + dmax = d; + v1 = i; + } + } + + thresh = (i_area/0.75) * 0.01 * i_factor; + + l_vertex[0] = 0; + + if(!wv1.getVertex(l_x_coord,l_y_coord, 0, v1,thresh)){ //if( get_vertex(marker_info2->x_coord, marker_info2->y_coord, 0, v1,thresh, wv1, &wvnum1) < 0 ) { + return false; + } + if(!wv2.getVertex(l_x_coord,l_y_coord,v1, this.coord_num-1, thresh)) {//if(get_vertex(marker_info2->x_coord, marker_info2->y_coord,v1, marker_info2->coord_num-1, thresh, wv2, &wvnum2) < 0 ) { + return false; + } + + if(wv1.number_of_vertex==1 && wv2.number_of_vertex==1) {//if( wvnum1 == 1 && wvnum2 == 1 ) { + l_vertex[1] = wv1.vertex[0]; + l_vertex[2] = v1; + l_vertex[3] = wv2.vertex[0]; + }else if( wv1.number_of_vertex>1 && wv2.number_of_vertex==0 ) {//}else if( wvnum1 > 1 && wvnum2 == 0 ) { + v2 = v1 / 2; + if(!wv1.getVertex(l_x_coord,l_y_coord,0, v2, thresh)) { + return false; + } + if(!wv2.getVertex(l_x_coord,l_y_coord,v2, v1, thresh)) { + return false; + } + if(wv1.number_of_vertex==1 && wv2.number_of_vertex==1 ) { + l_vertex[1] = wv1.vertex[0]; + l_vertex[2] = wv2.vertex[0]; + l_vertex[3] = v1; + }else{ + return false; + } + }else if(wv1.number_of_vertex==0 && wv2.number_of_vertex> 1 ) { + v2 = (v1 + this.coord_num-1) / 2; + + if(!wv1.getVertex(l_x_coord,l_y_coord,v1, v2, thresh)) { + return false; + } + if(!wv2.getVertex(l_x_coord,l_y_coord,v2, this.coord_num-1, thresh)) { + return false; + } + if( wv1.number_of_vertex==1 && wv2.number_of_vertex==1 ) { + l_vertex[1] = v1; + l_vertex[2] = wv1.vertex[0]; + l_vertex[3] = wv2.vertex[0]; + } + else { + return false; + } + } + else { + return false; + } + l_vertex[4] = this.coord_num-1;//この値使ってるの？ + // + area = i_area; + pos[0] = i_pos_x; + pos[1] = i_pos_y; +// marker_holder[marker_num2].pos[1] = wpos[i*2+1]; + return true; + } } +/** + * get_vertex関数を切り離すためのクラス + * + */ +final class NyARVertexCounter +{ + public final int[] vertex=new int[10];//5まで削れる + public int number_of_vertex; + private double thresh; + private int[] x_coord; + private int[] y_coord; + + public boolean getVertex(int[] i_x_coord, int[] i_y_coord, int st, int ed,double i_thresh) + { + this.number_of_vertex=0; + this.thresh=i_thresh; + this.x_coord=i_x_coord; + this.y_coord=i_y_coord; + return get_vertex(st,ed); + } + /** + * static int get_vertex( int x_coord[], int y_coord[], int st, int ed,double thresh, int vertex[], int *vnum) + * 関数の代替関数 + * @param x_coord + * @param y_coord + * @param st + * @param ed + * @param thresh + * @return + */ + private boolean get_vertex(int st, int ed) + { + double d, dmax; + double a, b, c; + int i, v1=0; + final int[] lx_coord=this.x_coord; + final int[] ly_coord=this.y_coord; + a = ly_coord[ed] - ly_coord[st]; + b = lx_coord[st] - lx_coord[ed]; + c = lx_coord[ed]*ly_coord[st] - ly_coord[ed]*lx_coord[st]; + dmax = 0; + for(i=st+1;i dmax ) { + dmax = d*d; + v1 = i; + } + } + if( dmax/(a*a+b*b) > thresh ) { + if(!get_vertex(st, v1)){ + return false; + } + if(number_of_vertex > 5 ){ + return false; + } + vertex[number_of_vertex] = v1;//vertex[(*vnum)] = v1; + number_of_vertex++;//(*vnum)++; + + if(!get_vertex(v1, ed)){ + return false; + } + } + return true; + } +} + diff --git a/src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java b/src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java new file mode 100644 index 0000000..6e03c2f --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java @@ -0,0 +1,148 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +/** + * このクラスは、detectMarkerがマーカーオブジェクトの蓄積に使うクラスです。 + * 実体を伴うマーカーホルダと、これを参照するマーカーアレイを持ちます。 + * + * マーカーアレイはマーカーホルダに存在するマーカーリストを特定の条件でフィルタした + * 結果を格納します。 + * + * 一度作られたマーカーホルダは繰り返し使用されます。 + * + * + */ +public class NyARMarkerList +{ + private int marker_holder_num;//marker_holderの使用中の数 + protected int marker_array_num;//marker_arrayの有効な数 + protected final NyARMarker[] marker_holder;//マーカーデータの保持配列 + protected final NyARMarker[] marker_array; //マーカーデータのインデックス配列 + /** + * 派生データ型をラップするときに使う + * @param i_holder + * 値の保持配列。全要素に実体を割り当てる必要がある。 + */ + protected NyARMarkerList(NyARMarker[] i_holder) + { + this.marker_holder=i_holder; + this.marker_array =new NyARMarker[i_holder.length]; + this.marker_array_num =0; + this.marker_holder_num =0; + } + public NyARMarkerList(int i_number_of_holder) + { + this.marker_holder=new NyARMarker[i_number_of_holder]; + //先にマーカーホルダにオブジェクトを作っておく + for(int i=0;i=this.marker_holder.length){ + throw new NyARException(); + } + return this.marker_holder[this.marker_holder_num]; + } + /** + * マーカーホルダの現在位置を1つ進めて、そのホルダを返す。 + * この関数を実行すると、使用中のマーカーホルダの数が1個増える。 + * @return + * 空いているマーカーホルダが無ければnullを返します。 + * + */ + public final NyARMarker getNextHolder() + { + //現在位置が終端位置ならnullを返す。 + if(this.marker_holder_num+1>=this.marker_holder.length){ + this.marker_holder_num=this.marker_holder.length; + return null; + } + this.marker_holder_num++; + return this.marker_holder[this.marker_holder_num]; + } + /** + * マーカーアレイのi_indexの要素を返す。 + * @param i_index + * @return + * @throws NyARException + */ + public final NyARMarker getMarker(int i_index) throws NyARException + { + if(i_index>=marker_array_num){ + throw new NyARException(); + } + return this.marker_array[i_index]; + } + /** + * マーカーアレイの要素数を返す。 + * @return + */ + public final int getMarkerNum() + { + return marker_array_num; + } + /** + * マーカーアレイの要素数と、マーカーホルダの現在位置をリセットする。 + * @return + */ + public final void reset() + { + this.marker_array_num=0; + this.marker_holder_num=0; + } + /** + * マーカーホルダに格納済みのマーカーから重なっているのものを除外して、 + * マーカーアレイにフィルタ結果を格納します。 + * [[この関数はマーカー検出処理と密接に関係する関数です。 + * NyARDetectMarkerクラス以外から呼び出さないで下さい。]] + * メモ:この関数はmarker_holderの内容を変化させまするので注意。 + */ + public final void updateMarkerArray() + { + //重なり処理かな？ + int i; + double d; + double[] pos_j,pos_i; +// NyARMarker[] marker_holder; + for(i=0; i < this.marker_holder_num; i++ ){ + pos_i=marker_holder[i].pos; + for(int j=i+1; j < this.marker_holder_num; j++ ) { + pos_j=marker_holder[j].pos; + d = (pos_i[0] - pos_j[0])*(pos_i[0] - pos_j[0])+ + (pos_i[1] - pos_j[1])*(pos_i[1] - pos_j[1]); + if(marker_holder[i].area >marker_holder[j].area ) { + if( d =0;i--){ + for(i2=clm-1;i2>=0;i2--){ m[i][i2]=0.0; } } } - public double[] getRowArray(int i_row) - { - return m[i_row]; - } + public double[][] getArray() { return m; } - public NyARVec getRowVec(int i_row) - { - return NyARVec.wrap(m[i_row]); - } +// public void getRowVec(int i_row,NyARVec o_vec) +// { +// o_vec.set(this.m[i_row],this.clm); +// } /** - * aとbの積をdestに出力する。arMatrixMul()の代替品 - * @param dest + * aとbの積を自分自身に格納する。arMatrixMul()の代替品 * @param a * @param b * @throws NyARException */ - public static void matrixMul(NyARMat dest, NyARMat a, NyARMat b) throws NyARException + public void matrixMul(NyARMat a, NyARMat b) throws NyARException { - if(a.clm != b.row || dest.row != a.row || dest.clm != b.clm){ + if(a.clm != b.row || this.row != a.row || this.clm != b.clm){ throw new NyARException(); } - for(int r = 0; r < dest.row; r++){ - for(int c = 0; c < dest.getClm(); c++){ - dest.m[r][c]=0.0;//dest.setARELEM0(r, c,0.0); - for(int i = 0; i < a.getClm(); i++){ - dest.m[r][c]+=a.m[r][i]*b.m[i][c];//ARELEM0(dest, r, c) += ARELEM0(a, r, i) * ARELEM0(b, i, c); + double w; + int r,c,i; + double[][] am=a.m,bm=b.m,dm=this.m; + //For順変更禁止 + for(r = 0; r < this.row; r++){ + for(c = 0; c < this.clm; c++){ + w=0.0;//dest.setARELEM0(r, c,0.0); + for(i = 0; i < a.clm; i++){ + w+=am[r][i]*bm[i][c];//ARELEM0(dest, r, c) += ARELEM0(a, r, i) * ARELEM0(b, i, c); } + dm[r][c]=w; } } } private int[] wk_nos_matrixSelfInv=new int[50]; + private final static double matrixSelfInv_epsl=1.0e-10; /** * i_targetを逆行列に変換する。arMatrixSelfInv()と、arMatrixSelfInv_minv()関数を合成してあります。 + * OPTIMIZE STEP[485->422] * @param i_target * 逆行列にする行列 * @throws NyARException */ public void matrixSelfInv() throws NyARException { - double[][] ap=m; - int dimen=ap.length; - double[] wcp,wap,wbp; + double[][] ap=this.m; + int dimen=this.row; + int dimen_1=dimen-1; + double[] ap_n,ap_ip,ap_i;//wap; int j,ip,nwork; int[] nos=wk_nos_matrixSelfInv;//この関数で初期化される。 - double epsl; + //double epsl; double p,pbuf,work; - epsl = 1.0e-10; /* Threshold value */ +// epsl = 1.0e-10; /* Threshold value */ /* check size */ switch(dimen){ case 0: @@ -141,20 +170,21 @@ public class NyARMat{ * ループ内で0初期化していいかが判らない。 */ ip=0; - int wap_ptr,wbp_ptr; +// int wap_ptr,wbp_ptr; + //For順変更禁止 for(int n=0; n=0; r--){ + for(c =this.clm-1;c>=0; c--) { - m[r][c]=source.m[r][c]; + dest_m[r][c]=src_m[r][c]; } } } public NyARMat matrixAllocDup() throws NyARException { - NyARMat result=new NyARMat(row,clm); - result.matrixDup(this); + NyARMat result=new NyARMat(this.row,this.clm); + //コピー + int r,c; + double[][] dest_m,src_m; + dest_m=result.m; + src_m =this.m; + //コピーはFor順を変えてもOK + for(r = this.row-1; r>=0; r--){ + for(c =this.clm-1;c>=0; c--) + { + dest_m[r][c]=src_m[r][c]; + } + } return result; } /** @@ -341,41 +389,39 @@ public class NyARMat{ private final static double PCA_VZERO=1e-16; //#define VZERO 1e-16 /** * static int EX( ARMat *input, ARVec *mean )の代替関数 + * Optimize:STEP:[144->110] * @param input * @param mean * @return * @throws NyARException */ - private static void PCA_EX(NyARMat input, NyARVec mean) throws NyARException + private void PCA_EX(NyARVec mean) throws NyARException { - double[] v; - - int row, clm; - - row = input.row; - clm = input.clm; - if(row <= 0 || clm <= 0){ + int lrow,lclm; + int i,i2; + lrow = this.row; + lclm = this.clm; + double lm[][]=this.m; + + if(lrow <= 0 || lclm <= 0){ throw new NyARException(); - } - if( mean.getClm() != clm ){ + } + if( mean.getClm() != lclm ){ throw new NyARException(); - } - double[] mean_array=mean.getArray(); - for(int i = 0; i < clm; i++ ){ - mean_array[i]=0.0;//mean->v[i] = 0.0; - } - - v=mean.getArray(); - for(int i = 0; i < row; i++ ) { - for(int j = 0; j < clm; j++ ){ - //*(v++) += *(m++); - v[j]+=input.m[i][j]; - } - } - - for(int i = 0; i < clm; i++ ){ - mean_array[i]/=row;//mean->v[i] /= row; - } + } +// double[] mean_array=mean.getArray(); +// mean.zeroClear(); + final double[] mean_array=mean.getArray(); + double w; + //For順変更禁止 + for(i2=0;i2v[i] /= row; + } } /** * static int CENTER( ARMat *inout, ARVec *mean )の代替関数 @@ -427,12 +473,12 @@ public class NyARMat{ for(int i = 0; i < row; i++ ) { for(int j = 0; j < row; j++ ) { if( j < i ) { - NyARException.trap("未チェックのパス");{ + NyARException.trap("未チェックのパス"); output.m[i][j]=output.m[j][i];//*out = output->m[j*row+i]; - } }else{ - in1=input.getRowArray(i);//in1 = &(input->m[clm*i]); - in2=input.getRowArray(j);//in2 = &(input->m[clm*j]); + NyARException.trap("未チェックのパス"); + in1=input.m[i];//input.getRowArray(i);//in1 = &(input->m[clm*i]); + in2=input.m[j];//input.getRowArray(j);//in2 = &(input->m[clm*j]); output.m[i][j]=0;//*out = 0.0; for(int k = 0; k < clm; k++ ){ output.m[i][j]+=(in1[k]*in2[k]);//*out += *(in1++) * *(in2++); @@ -444,148 +490,165 @@ public class NyARMat{ } /** * static int xt_by_x( ARMat *input, ARMat *output )の代替関数 + * Optimize:2008.04.19 * @param input - * @param output + * @param i_output * @throws NyARException */ - private static void PCA_xt_by_x(NyARMat input, NyARMat output) throws NyARException + private static void PCA_xt_by_x(NyARMat input, NyARMat i_output) throws NyARException { double[] in; int row, clm; row = input.row; clm = input.clm; - if(output.row!= clm || output.clm != clm ){ + if(i_output.row!= clm || i_output.clm != clm ){ throw new NyARException(); } - + + int k,j; + double[][] out_m=i_output.m; + double w; for(int i = 0; i < clm; i++ ) { - for(int j = 0; j < clm; j++ ) { + for(j = 0; j < clm; j++ ) { if( j < i ) { - output.m[i][j]=output.m[j][i];//*out = output->m[j*clm+i]; + out_m[i][j]=out_m[j][i];//*out = output->m[j*clm+i]; }else{ - output.m[i][j]=0.0;//*out = 0.0; - for(int k = 0; k < row; k++ ){ - in=input.getRowArray(k); - output.m[i][j]+=(in[i]*in[j]);//*out += *in1 * *in2; + w=0.0;//*out = 0.0; + for(k = 0; k < row; k++ ){ + in=input.m[k];//in=input.getRowArray(k); + w+=(in[i]*in[j]);//*out += *in1 * *in2; } + out_m[i][j]=w; } } } } + private final NyARVec wk_PCA_QRM_ev=new NyARVec(1); /** * static int QRM( ARMat *a, ARVec *dv )の代替関数 * @param a * @param dv * @throws NyARException */ - private static void PCA_QRM(NyARMat a, NyARVec dv) throws NyARException + private void PCA_QRM(NyARVec dv) throws NyARException { - double w, t, s, x, y, c; - int dim, iter; - double[] dv_array=dv.getArray(); - - dim = a.row; - if( dim != a.clm || dim < 2 ){ + double w, t, s, x, y, c; + int dim, iter; + double[] dv_array=dv.getArray(); + + dim = this.row; + if( dim != this.clm || dim < 2 ){ throw new NyARException(); - } - if( dv.getClm() != dim ){ + } + if( dv.getClm() != dim ){ throw new NyARException(); - } - - NyARVec ev = new NyARVec( dim ); - double[] ev_array=ev.getArray(); - if( ev == null ){ + } + + NyARVec ev = this.wk_PCA_QRM_ev; + ev.realloc(dim); + double[] ev_array=ev.getArray(); + if( ev == null ){ throw new NyARException(); - } + } - NyARVec.vecTridiagonalize(a,dv,ev,1); - - ev_array[0]=0.0;//ev->v[0] = 0.0; - for(int h = dim-1; h > 0; h-- ) { - int j = h; - while(j>0 && Math.abs(ev_array[j]) > PCA_EPS*(Math.abs(dv_array[j-1])+Math.abs(dv_array[j]))){// while(j>0 && fabs(ev->v[j]) > EPS*(fabs(dv->v[j-1])+fabs(dv->v[j]))) j--; - j--; - } - if( j == h ){ - continue; - } - iter = 0; - do{ - iter++; - if( iter > PCA_MAX_ITER ){ - break; - } - w = (dv_array[h-1] - dv_array[h]) / 2;//w = (dv->v[h-1] - dv->v[h]) / 2;//ここ？ - t = ev_array[h] * ev_array[h];//t = ev->v[h] * ev->v[h]; - s = Math.sqrt(w*w+t); - if( w < 0 ){ - s = -s; - } - x=dv_array[j] - dv_array[h] + t/(w+s);//x = dv->v[j] - dv->v[h] + t/(w+s); - y=ev_array[j+1];//y = ev->v[j+1]; - for(int k = j; k < h; k++ ){ - if( Math.abs(x) >= Math.abs(y)){ - if( Math.abs(x) > PCA_VZERO ) { - t = -y / x; - c = 1 / Math.sqrt(t*t+1); - s = t * c; - }else{ - c = 1.0; - s = 0.0; - } - }else{ - t = -x / y; - s = 1.0 / Math.sqrt(t*t+1); - c = t * s; - } - w = dv_array[k] - dv_array[k+1];//w = dv->v[k] - dv->v[k+1]; - t = (w * s + 2 * c * ev_array[k+1]) * s;//t = (w * s + 2 * c * ev->v[k+1]) * s; - dv_array[k]-=t;//dv->v[k] -= t; - dv_array[k+1]+=t;//dv->v[k+1] += t; - if( k > j){ - NyARException.trap("未チェックパス");{ - ev_array[k]=c * ev_array[k] - s * y;//ev->v[k] = c * ev->v[k] - s * y; - } - } - ev_array[k+1]+=s * (c * w - 2 * s * ev_array[k+1]);//ev->v[k+1] += s * (c * w - 2 * s * ev->v[k+1]); - - for(int i = 0; i < dim; i++ ){ - x = a.m[k][i];//x = a->m[k*dim+i]; - y = a.m[k+1][i];//y = a->m[(k+1)*dim+i]; - a.m[k][i]=c * x - s * y;//a->m[k*dim+i] = c * x - s * y; - a.m[k+1][i]=s * x + c * y;//a->m[(k+1)*dim+i] = s * x + c * y; - } - if( k < h-1 ) { - NyARException.trap("未チェックパス");{ - x = ev_array[k+1];//x = ev->v[k+1]; - y =-s*ev_array[k+2];//y = -s * ev->v[k+2]; - ev_array[k+2]*=c;//ev->v[k+2] *= c; - } - } - } - }while(Math.abs(ev_array[h]) > PCA_EPS*(Math.abs(dv_array[h-1])+Math.abs(dv_array[h]))); - } - double[] v1,v2; - for(int k = 0; k < dim-1; k++ ) { - int h = k; - t=dv_array[h];//t = dv->v[h]; - for(int i = k+1; i < dim; i++ ){ - if(dv_array[i] > t ){//if( dv->v[i] > t ) { - h = i; - t=dv_array[h];//t = dv->v[h]; - } - } - dv_array[h]=dv_array[k];//dv->v[h] = dv->v[k]; - dv_array[k]=t;//dv->v[k] = t; - v1=a.getRowArray(h);//v1 = &(a->m[h*dim]); - v2=a.getRowArray(k);//v2 = &(a->m[k*dim]); - for(int i = 0; i < dim; i++ ) { - w=v1[i];//w = *v1; - v1[i]=v2[i];//*v1 = *v2; - v2[i]=w;//*v2 = w; - } - } + this.vecTridiagonalize(dv,ev,1); + + ev_array[0]=0.0;//ev->v[0] = 0.0; + for(int h = dim-1; h > 0; h-- ) { + int j = h; + while(j>0 && Math.abs(ev_array[j]) > PCA_EPS*(Math.abs(dv_array[j-1])+Math.abs(dv_array[j]))){// while(j>0 && fabs(ev->v[j]) > EPS*(fabs(dv->v[j-1])+fabs(dv->v[j]))) j--; + j--; + } + if( j == h ){ + continue; + } + iter = 0; + do{ + iter++; + if( iter > PCA_MAX_ITER ){ + break; + } + w = (dv_array[h-1] - dv_array[h]) / 2;//w = (dv->v[h-1] - dv->v[h]) / 2;//ここ？ + t = ev_array[h] * ev_array[h];//t = ev->v[h] * ev->v[h]; + s = Math.sqrt(w*w+t); + if( w < 0 ){ + s = -s; + } + x=dv_array[j] - dv_array[h] + t/(w+s);//x = dv->v[j] - dv->v[h] + t/(w+s); + y=ev_array[j+1];//y = ev->v[j+1]; + for(int k = j; k < h; k++ ){ + if( Math.abs(x) >= Math.abs(y)){ + if( Math.abs(x) > PCA_VZERO ) { + t = -y / x; + c = 1 / Math.sqrt(t*t+1); + s = t * c; + }else{ + c = 1.0; + s = 0.0; + } + }else{ + t = -x / y; + s = 1.0 / Math.sqrt(t*t+1); + c = t * s; + } + w = dv_array[k] - dv_array[k+1];//w = dv->v[k] - dv->v[k+1]; + t = (w * s + 2 * c * ev_array[k+1]) * s;//t = (w * s + 2 * c * ev->v[k+1]) * s; + dv_array[k]-=t;//dv->v[k] -= t; + dv_array[k+1]+=t;//dv->v[k+1] += t; + if( k > j){ + NyARException.trap("未チェックパス");{ + ev_array[k]=c * ev_array[k] - s * y;//ev->v[k] = c * ev->v[k] - s * y; + } + } + ev_array[k+1]+=s * (c * w - 2 * s * ev_array[k+1]);//ev->v[k+1] += s * (c * w - 2 * s * ev->v[k+1]); + + for(int i = 0; i < dim; i++ ){ + x = this.m[k][i];//x = a->m[k*dim+i]; + y = this.m[k+1][i];//y = a->m[(k+1)*dim+i]; + this.m[k][i]=c * x - s * y;//a->m[k*dim+i] = c * x - s * y; + this.m[k+1][i]=s * x + c * y;//a->m[(k+1)*dim+i] = s * x + c * y; + } + if( k < h-1 ) { + NyARException.trap("未チェックパス");{ + x = ev_array[k+1];//x = ev->v[k+1]; + y =-s*ev_array[k+2];//y = -s * ev->v[k+2]; + ev_array[k+2]*=c;//ev->v[k+2] *= c; + } + } + } + }while(Math.abs(ev_array[h]) > PCA_EPS*(Math.abs(dv_array[h-1])+Math.abs(dv_array[h]))); + } + for(int k = 0; k < dim-1; k++ ) { + int h = k; + t=dv_array[h];//t = dv->v[h]; + for(int i = k+1; i < dim; i++ ){ + if(dv_array[i] > t ){//if( dv->v[i] > t ) { + h = i; + t=dv_array[h];//t = dv->v[h]; + } + } + dv_array[h]=dv_array[k];//dv->v[h] = dv->v[k]; + dv_array[k]=t;//dv->v[k] = t; + this.flipRow(h,k); + } + } + /** + * i_row_1番目の行と、i_row_2番目の行を入れ替える。 + * @param i_row_1 + * @param i_row_2 + */ + private void flipRow(int i_row_1,int i_row_2) + { + int i; + double w; + double[] r1=this.m[i_row_1],r2=this.m[i_row_2]; + //For順変更OK + for(i=clm-1;i>=0;i--){ + w=r1[i]; + r1[i]=r2[i]; + r2[i]=w; + } } /** * static int EV_create( ARMat *input, ARMat *u, ARMat *output, ARVec *ev )の代替関数 @@ -617,8 +680,9 @@ public class NyARMat{ double[] m1,ev_array; double sum, work; - m =output.getArray();//m = output->m; - in=input.getArray(); + NyARException.trap("未チェックのパス"); + m =output.m;//m = output->m; + in=input.m; int i; ev_array=ev.getArray(); for(i = 0; i < row; i++ ) { @@ -630,7 +694,7 @@ public class NyARMat{ work = 1 / Math.sqrt(Math.abs(ev_array[i]));//work = 1 / sqrt(fabs(ev->v[i])); for(int j = 0; j < clm; j++ ) { sum = 0.0; - m1=u.getRowArray(i);//m1 = &(u->m[i*row]); + m1=u.m[i];//m1 = &(u->m[i*row]); // m2=input.getPointer(j);//m2 = &(input->m[j]); for(int k = 0; k < row; k++ ) { sum+=m1[k]+in[k][j];//sum += *m1 * *m2; @@ -653,46 +717,61 @@ public class NyARMat{ } } } - /*static int PCA( ARMat *input, ARMat *output, ARVec *ev )*/ - private static void PCA_PCA(NyARMat input, NyARMat output, NyARVec ev) throws NyARException + private NyARMat wk_PCA_PCA_u=null; + /** + * static int PCA( ARMat *input, ARMat *output, ARVec *ev ) + * + * @param output + * @param o_ev + * @throws NyARException + */ + private void PCA_PCA(NyARMat o_output, NyARVec o_ev) throws NyARException { - NyARMat u; - int row, clm, min; - double[] ev_array=ev.getArray(); + + int l_row, l_clm, min; + double[] ev_array=o_ev.getArray(); - row =input.row;//row = input->row; - clm =input.clm;//clm = input->clm; - min =(clm < row)? clm: row; - if( row < 2 || clm < 2 ){ + l_row =this.row;//row = input->row; + l_clm =this.clm;//clm = input->clm; + min =(l_clm < l_row)? l_clm: l_row; + if( l_row < 2 || l_clm < 2 ){ throw new NyARException(); } - if( output.clm != input.clm){//if( output->clm != input->clm ){ + if( o_output.clm != this.clm){//if( output->clm != input->clm ){ throw new NyARException(); } - if( output.row!= min ){//if( output->row != min ){ + if( o_output.row!= min ){//if( output->row != min ){ throw new NyARException(); } - if( ev.getClm() != min ){//if( ev->clm != min ){ + if( o_ev.getClm() != min ){//if( ev->clm != min ){ throw new NyARException(); } - u =new NyARMat( min, min ); + + NyARMat u;// u =new NyARMat( min, min ); + if(this.wk_PCA_PCA_u==null){ + u=new NyARMat( min, min ); + this.wk_PCA_PCA_u=u; + }else{ + u=this.wk_PCA_PCA_u; + u.realloc(min,min); + } + - if( row < clm ){ + if( l_row < l_clm ){ NyARException.trap("未チェックのパス"); - PCA_x_by_xt( input, u );//if(x_by_xt( input, u ) < 0 ) { + PCA_x_by_xt( this, u );//if(x_by_xt( input, u ) < 0 ) { }else{ - PCA_xt_by_x( input, u );//if(xt_by_x( input, u ) < 0 ) { + PCA_xt_by_x( this, u );//if(xt_by_x( input, u ) < 0 ) { } - PCA_QRM( u, ev ); + u.PCA_QRM(o_ev); double[][] m1,m2; - if( row < clm ) { - NyARException.trap("未チェックのパス");{ - PCA_EV_create( input, u, output, ev ); - } + if( l_row < l_clm ) { + NyARException.trap("未チェックのパス"); + PCA_EV_create( this, u, o_output, o_ev ); }else{ m1=u.m;//m1 = u->m; - m2=output.m;//m2 = output->m; + m2=o_output.m;//m2 = output->m; int i; for(i = 0; i < min; i++){ if( ev_array[i] < PCA_VZERO){//if( ev->v[i] < VZERO ){ @@ -711,53 +790,76 @@ public class NyARMat{ } } } - - /*int arMatrixPCA( ARMat *input, ARMat *evec, ARVec *ev, ARVec *mean );*/ - public static void matrixPCA(NyARMat input, NyARMat evec, NyARVec ev, NyARVec mean) throws NyARException + private NyARMat wk_work_matrixPCA=null; + /** + * int arMatrixPCA( ARMat *input, ARMat *evec, ARVec *ev, ARVec *mean ); + * 関数の置き換え。input引数がthisになる。 + * Optimize:2008.04.19 + * @param o_evec + * @param o_ev + * + * @param mean + * @throws NyARException + */ + public void matrixPCA(NyARMat o_evec, NyARVec o_ev, NyARVec mean) throws NyARException { - NyARMat work; double srow, sum; - int row, clm; + int l_row, l_clm; int check; - - row=input.row;//row = input->row; - clm=input.clm;//clm = input->clm; - check = (row < clm)? row: clm; - if( row < 2 || clm < 2 ){ + + + l_row=this.row;//row = input->row; + l_clm=this.clm;//clm = input->clm; + check = (l_row < l_clm)? l_row: l_clm; + if( l_row < 2 || l_clm < 2 ){ throw new NyARException(); } - if( evec.clm != input.clm || evec.row != check ){//if( evec->clm != input->clm || evec->row != check ){ + if( o_evec.clm != l_clm || o_evec.row != check ){//if( evec->clm != input->clm || evec->row != check ){ throw new NyARException(); } - if( ev.getClm() != check ){//if( ev->clm != check ){ + if( o_ev.getClm() != check ){//if( ev->clm != check ){ throw new NyARException(); } - if( mean.getClm() != input.clm){//if( mean->clm != input->clm ){ + if( mean.getClm() != l_clm){//if( mean->clm != input->clm ){ throw new NyARException(); } + + //自分の内容をワークにコピー(高速化の為に、1度作ったインスタンスは使いまわす) + NyARMat work; + if(this.wk_work_matrixPCA==null){ + work=this.matrixAllocDup(); + this.wk_work_matrixPCA=work; + }else{ + work=this.wk_work_matrixPCA; + work.matrixDup(this);//arMatrixAllocDup( input );work = arMatrixAllocDup( input ); + } + - work =input.matrixAllocDup();//arMatrixAllocDup( input );work = arMatrixAllocDup( input ); - - srow = Math.sqrt((double)row); - PCA_EX( work, mean ); + srow = Math.sqrt((double)l_row); + work.PCA_EX(mean ); PCA_CENTER(work,mean); - for(int i=0; im[i] /= srow; } } - PCA_PCA( work, evec, ev ); + work.PCA_PCA(o_evec, o_ev); sum = 0.0; - double[] ev_array=ev.getArray(); - for(int i = 0; i < ev.getClm(); i++ ){//for(int i = 0; i < ev->clm; i++ ){ + double[] ev_array=o_ev.getArray(); + int ev_clm=o_ev.getClm(); + //For順変更禁止 + for(i=0;iclm; i++ ){ sum+=ev_array[i];//sum += ev->v[i]; } - for(int i = 0; i < ev.getClm(); i++ ){//for(int i = 0; i < ev->clm; i++ ){ + //For順変更禁止 + for(i=0;iclm; i++ ){ ev_array[i]/=sum;//ev->v[i] /= sum; } } @@ -789,7 +891,7 @@ public class NyARMat{ work =input.matrixAllocDup(); NyARException.trap("未チェックパス"); - PCA_PCA( work, evec, ev );//rval = PCA( work, evec, ev ); + work.PCA_PCA(evec, ev );//rval = PCA( work, evec, ev ); sum = 0.0; double[] ev_array=ev.getArray(); for(int i = 0; i < ev.getClm(); i++ ){//for( i = 0; i < ev->clm; i++ ){ @@ -807,7 +909,7 @@ public class NyARMat{ NyARException.trap("未チェックのパス"); NyARMat dest=new NyARMat(a.row, b.clm); NyARException.trap("未チェックのパス"); - matrixMul(dest, a, b); + dest.matrixMul(a, b); return dest; } /*static double mdet(double *ap, int dimen, int rowa)*/ @@ -860,4 +962,122 @@ public class NyARMat{ } return Det_mdet(m.getArray(), m.row, m.clm);//return mdet(m->m, m->row, m->row); } + private final NyARVec wk_vecTridiagonalize_vec=new NyARVec(0); + private final NyARVec wk_vecTridiagonalize_vec2=new NyARVec(0); + /** + * arVecTridiagonalize関数の代替品 + * a,d,e間で演算をしてる。何をどうしているかはさっぱりさっぱり + * @param a + * @param d + * @param e + * @param i_e_start + * 演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる) + * @return + * @throws NyARException + */ + private void vecTridiagonalize(NyARVec d, NyARVec e,int i_e_start) throws NyARException + { + NyARVec vec=wk_vecTridiagonalize_vec; + //double[][] a_array=a.getArray(); + double s, t, p, q; + int dim; + + if(this.clm!=this.row){//if(a.getClm()!=a.getRow()){ + throw new NyARException(); + } + if(this.clm != d.getClm()){//if(a.getClm() != d.clm){ + throw new NyARException(); + } + if(this.clm != e.getClm()){//if(a.getClm() != e.clm){ + throw new NyARException(); + } + dim = this.getClm(); + + double[] d_vec,e_vec; + d_vec=d.getArray(); + e_vec=e.getArray(); + double[] a_vec_k; + + for(int k = 0; k < dim-2; k++ ){ + + a_vec_k=this.m[k]; + vec.setNewArray(a_vec_k,clm);//vec=this.getRowVec(k);//double[] vec_array=vec.getArray(); + NyARException.trap("未チェックパス"); + d_vec[k]=a_vec_k[k];//d.v[k]=vec.v[k];//d.set(k,v.get(k)); //d->v[k] = v[k]; + + //wv1.clm = dim-k-1; + //wv1.v = &(v[k+1]); + NyARException.trap("未チェックパス"); + e_vec[k+i_e_start]=vec.vecHousehold(k+1);//e.v[k+i_e_start]=vec.vecHousehold(k+1);//e->v[k] = arVecHousehold(&wv1); + if(e_vec[k+i_e_start]== 0.0 ){//if(e.v[k+i_e_start]== 0.0 ){//if(e.v[k+i_e_start]== 0.0 ){ + continue; + } + + for(int i = k+1; i < dim; i++ ){ + s = 0.0; + for(int j = k+1; j < i; j++ ) { + NyARException.trap("未チェックのパス"); + s += this.m[j][i] * a_vec_k[j];//s += a_array[j][i] * vec.v[j];//s += a.get(j*dim+i) * v.get(j);//s += a->m[j*dim+i] * v[j]; + } + for(int j = i; j < dim; j++ ) { + NyARException.trap("未チェックのパス"); + s += this.m[i][j] * a_vec_k[j];//s += a_array[i][j] * vec.v[j];//s += a.get(i*dim+j) * v.get(j);//s += a->m[i*dim+j] * v[j]; + } + NyARException.trap("未チェックのパス"); + d_vec[i]=s;//d.v[i]=s;//d->v[i] = s; + } + + + //wv1.clm = wv2.clm = dim-k-1; + //wv1.v = &(v[k+1]); + //wv2.v = &(d->v[k+1]); + a_vec_k=this.m[k]; + vec.setNewArray(a_vec_k,clm);//vec=this.getRowVec(k); +// vec_array=vec.getArray(); + NyARException.trap("未チェックパス"); + t = vec.vecInnerproduct(d,k+1)/ 2; + for(int i = dim-1; i > k; i-- ) { + NyARException.trap("未チェックパス"); + p = a_vec_k[i];//p = v.get(i);//p = v[i]; + d_vec[i]-=t*p;q=d_vec[i];//d.v[i]-=t*p;q=d.v[i];//q = d->v[i] -= t*p； + for(int j = i; j < dim; j++ ){ + NyARException.trap("未チェックパス"); + this.m[i][j]-=p*(d_vec[j] + q*a_vec_k[j]);//a.m[i][j]-=p*(d.v[j] + q*vec.v[j]);//a->m[i*dim+j] -= p*(d->v[j]) + q*v[j]; + } + } + } + + if( dim >= 2) { + d_vec[dim-2]=this.m[dim-2][dim-2];//d.v[dim-2]=a.m[dim-2][dim-2];//d->v[dim-2] = a->m[(dim-2)*dim+(dim-2)]; + e_vec[dim-2+i_e_start]=this.m[dim-2][dim-1];//e.v[dim-2+i_e_start]=a.m[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)]; + } + + if( dim >= 1 ){ + d_vec[dim-1]=this.m[dim-1][dim-1];//d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] = a->m[(dim-1)*dim+(dim-1)]; + } + NyARVec vec2=this.wk_vecTridiagonalize_vec2; + for(int k = dim-1; k >= 0; k--) { + a_vec_k=this.m[k]; + vec.setNewArray(a_vec_k,clm);//vec=this.getRowVec(k);//v = a.getPointer(k*dim);//v = &(a->m[k*dim]); + if( k < dim-2 ) { + for(int i = k+1; i < dim; i++ ){ + //wv1.clm = wv2.clm = dim-k-1; + //wv1.v = &(v[k+1]); + //wv2.v = &(a->m[i*dim+k+1]); + vec2.setNewArray(this.m[i],clm);//vec2=this.getRowVec(i); + + t = vec.vecInnerproduct(vec2,k+1); + for(int j = k+1; j < dim; j++ ){ + NyARException.trap("未チェックパス"); + this.m[i][j]-=t*a_vec_k[j];//a_array[i][j]-=t*vec.v[j];//a.subValue(i*dim+j,t*v.get(j));//a->m[i*dim+j] -= t * v[j]; + } + } + } + for(int i = 0; i < dim; i++ ){ + a_vec_k[i]=0.0;//v.set(i,0.0);//v[i] = 0.0; + } + a_vec_k[k]=1;//v.set(k,1);//v[k] = 1; + } + return; + } } \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARParam.java b/src/jp/nyatla/nyartoolkit/core/NyARParam.java index 73a41cc..f9d4014 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARParam.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARParam.java @@ -47,8 +47,8 @@ public class NyARParam{ private static final int SIZE_OF_PARAM_SET=4+4+(3*4*8)+(4*8); private static final int PD_LOOP = 3; protected int xsize, ysize; - private double[][] mat=new double[3][4];//Double2dArray mat=new Double2dArray(3,4); - private double[] dist_factor=new double[4]; + private double[] array34=new double[3*4];//Double2dArray mat=new Double2dArray(3,4); + private double[] dist_factor=new double[4]; public int getX() { return xsize; @@ -61,9 +61,13 @@ public class NyARParam{ { return dist_factor; } - public double[][] getMat() + /** + * パラメタを格納した[4x3]配列を返します。 + * @return + */ + public final double[] get34Array() { - return mat; + return array34; } /** * ARToolKit標準ファイルから1個目の設定をロードする。 @@ -85,25 +89,26 @@ public class NyARParam{ i_stream.close(); xsize = new_inst[0].xsize; ysize = new_inst[0].ysize; - mat = new_inst[0].mat; + array34 = new_inst[0].array34; dist_factor = new_inst[0].dist_factor; } catch (Exception e) { throw new NyARException(e); } } /*static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )*/ - private static double dot( double a1, double a2, double a3,double b1, double b2, double b3 ) + private final static double dot( double a1, double a2, double a3,double b1, double b2, double b3 ) { return( a1 * b1 + a2 * b2 + a3 * b3 ); } /* static double norm( double a, double b, double c )*/ - private static double norm( double a, double b, double c ) + private final static double norm( double a, double b, double c ) { return Math.sqrt( a*a + b*b + c*c ); } /** * int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] ); * 関数の置き換え + * Optimize STEP[754->665] * @param o_cpara * 戻り引数。3x4のマトリクスを指定すること。 * @param o_trans @@ -112,22 +117,32 @@ public class NyARParam{ */ public void decompMat(NyARMat o_cpara, NyARMat o_trans) { - double[][] source=mat; - double[][] Cpara=new double[3][4];//double Cpara[3][4]; + double[] source=array34; + double[] Cpara=new double[3*4];//double Cpara[3][4]; double rem1, rem2, rem3; - - if(source[2][3]>= 0 ){//if( source[2][3] >= 0 ) { - for(int r = 0; r < 3; r++ ){ - for(int c = 0; c < 4; c++ ){ - Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c]; - } + int i; + if(source[2*4+3]>= 0 ){//if( source[2][3] >= 0 ) { + // + //for(int r = 0; r < 3; r++ ){ + // for(int c = 0; c < 4; c++ ){ + // Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c]; + // } + //} + for(i=0;i<12;i++){ + Cpara[i]=source[i];//Cpara[r][c] = source[r][c]; } + // }else { - for(int r = 0; r < 3; r++ ){ - for(int c = 0; c < 4; c++ ){ - Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]); - } + // + //for(int r = 0; r < 3; r++ ){ + // for(int c = 0; c < 4; c++ ){ + // Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]); + // } + //} + for(i=0;i<12;i++){ + Cpara[i]=source[i];//Cpara[r][c] = source[r][c]; } + // } double[][] cpara=o_cpara.getArray(); @@ -137,33 +152,33 @@ public class NyARParam{ cpara[r][c]=0.0;//cpara[r][c] = 0.0; } } - cpara[2][2]=norm(Cpara[2][0],Cpara[2][1], Cpara[2][2]);//cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] ); - trans[2][0]=Cpara[2][0] / cpara[2][2];//trans[2][0] = Cpara[2][0] / cpara[2][2]; - trans[2][1]=Cpara[2][1]/ cpara[2][2];//trans[2][1] = Cpara[2][1] / cpara[2][2]; - trans[2][2]=Cpara[2][2]/ cpara[2][2];//trans[2][2] = Cpara[2][2] / cpara[2][2]; - trans[2][3]=Cpara[2][3] / cpara[2][2];//trans[2][3] = Cpara[2][3] / cpara[2][2]; + cpara[2][2]=norm(Cpara[2*4+0],Cpara[2*4+1], Cpara[2*4+2]);//cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] ); + trans[2][0]=Cpara[2*4+0] / cpara[2][2];//trans[2][0] = Cpara[2][0] / cpara[2][2]; + trans[2][1]=Cpara[2*4+1]/ cpara[2][2];//trans[2][1] = Cpara[2][1] / cpara[2][2]; + trans[2][2]=Cpara[2*4+2]/ cpara[2][2];//trans[2][2] = Cpara[2][2] / cpara[2][2]; + trans[2][3]=Cpara[2*4+3] / cpara[2][2];//trans[2][3] = Cpara[2][3] / cpara[2][2]; - cpara[1][2]=dot(trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2]);//cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2] ); - rem1=Cpara[1][0]- cpara[1][2] * trans[2][0];//rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0]; - rem2=Cpara[1][1] - cpara[1][2] * trans[2][1];//rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1]; - rem3=Cpara[1][2] - cpara[1][2] * trans[2][2];//rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2]; + cpara[1][2]=dot(trans[2][0], trans[2][1], trans[2][2],Cpara[1*4+0], Cpara[1*4+1], Cpara[1*4+2]);//cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2] ); + rem1=Cpara[1*4+0]- cpara[1][2] * trans[2][0];//rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0]; + rem2=Cpara[1*4+1] - cpara[1][2] * trans[2][1];//rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1]; + rem3=Cpara[1*4+2] - cpara[1][2] * trans[2][2];//rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2]; cpara[1][1]=norm(rem1, rem2, rem3 );//cpara[1][1] = norm( rem1, rem2, rem3 ); trans[1][0]= rem1/cpara[1][1];//trans[1][0] = rem1 / cpara[1][1]; trans[1][1]=rem2/cpara[1][1];//trans[1][1] = rem2 / cpara[1][1]; trans[1][2]=rem3 / cpara[1][1];//trans[1][2] = rem3 / cpara[1][1]; - cpara[0][2]=dot(trans[2][0],trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );//cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); - cpara[0][1]=dot(trans[1][0],trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2]);//cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); - rem1=Cpara[0][0]- cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];//rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0]; - rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];//rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1]; - rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];//rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2]; + cpara[0][2]=dot(trans[2][0],trans[2][1], trans[2][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2] );//cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); + cpara[0][1]=dot(trans[1][0],trans[1][1], trans[1][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2]);//cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); + rem1=Cpara[0*4+0]- cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];//rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0]; + rem2 = Cpara[0*4+1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];//rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1]; + rem3 = Cpara[0*4+2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];//rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2]; cpara[0][0]=norm(rem1, rem2, rem3);//cpara[0][0] = norm( rem1, rem2, rem3 ); trans[0][0]=rem1 / cpara[0][0];//trans[0][0] = rem1 / cpara[0][0]; trans[0][1]= rem2 / cpara[0][0];//trans[0][1] = rem2 / cpara[0][0]; trans[0][2]= rem3 / cpara[0][0];//trans[0][2] = rem3 / cpara[0][0]; - trans[1][3]=(Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1];//trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1]; - trans[0][3]=(Cpara[0][3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0];//trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0]; + trans[1][3]=(Cpara[1*4+3] - cpara[1][2]*trans[2][3]) / cpara[1][1];//trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1]; + trans[0][3]=(Cpara[0*4+3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0];//trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0]; for(int r = 0; r < 3; r++ ){ for(int c = 0; c < 3; c++ ){ @@ -181,7 +196,7 @@ public class NyARParam{ System.out.println("Distortion factor = "+dist_factor[0]+" "+dist_factor[1]+" "+dist_factor[2]+" "+dist_factor[3]);//printf("Distortion factor = %f %f %f %f\n", param->dist_factor[0],param->dist_factor[1], param->dist_factor[2], param->dist_factor[3] ); for(int j = 0; j < 3; j++ ) {//for(j = 0; j < 3; j++ ) { for(int i = 0; i < 4; i++ ){ - System.out.print(mat[j][i]+" ");//printf("%7.5f ", param->mat[j][i]); + System.out.print(array34[j*4+i]+" ");//printf("%7.5f ", param->mat[j][i]); } System.out.println();// printf("\n"); }//} @@ -215,9 +230,9 @@ public class NyARParam{ scale = (double)i_xsize / (double)(xsize);//scale = (double)xsize / (double)(source->xsize); for(int i = 0; i < 4; i++ ) { - mat[0][i]=mat[0][i]*scale;//newparam->mat[0][i] = source->mat[0][i] * scale; - mat[1][i]=mat[1][i]*scale;//newparam->mat[1][i] = source->mat[1][i] * scale; - mat[2][i]=mat[2][i];//newparam->mat[2][i] = source->mat[2][i]; + array34[0*4+i]=array34[0*4+i]*scale;//newparam->mat[0][i] = source->mat[0][i] * scale; + array34[1*4+i]=array34[1*4+i]*scale;//newparam->mat[1][i] = source->mat[1][i] * scale; + array34[2*4+i]=array34[2*4+i];//newparam->mat[2][i] = source->mat[2][i]; } dist_factor[0] = dist_factor[0] * scale;//newparam->dist_factor[0] = source->dist_factor[0] * scale; @@ -238,27 +253,44 @@ public class NyARParam{ */ public void ideal2Observ(double ix,double iy,DoubleValue ox, DoubleValue oy) { - double x, y, d; - - x = (ix - dist_factor[0]) * dist_factor[3]; - y = (iy - dist_factor[1]) * dist_factor[3]; - if( x == 0.0 && y == 0.0 ) { - ox.value=dist_factor[0]; - oy.value=dist_factor[1]; - }else{ - d = 1.0 - dist_factor[2]/100000000.0 * (x*x+y*y); - ox.value=x * d + dist_factor[0]; - oy.value=y * d + dist_factor[1]; - } + + double x, y, d; + final double d0,d1,d3; + final double df[]=this.dist_factor; + d0=df[0]; + d1=df[1]; + d3=df[3]; + x = (ix - d0) * d3; + y = (iy - d1) * d3; + if( x == 0.0 && y == 0.0 ) { + ox.value=d0; + oy.value=d1; + }else{ + d = 1.0 - df[2]/100000000.0 * (x*x+y*y); + ox.value=x * d + d0; + oy.value=y * d + d1; + } } - /*int arParamObserv2Ideal( const double dist_factor[4], const double ox, const double oy,double *ix, double *iy );*/ + /** + * int arParamObserv2Ideal( const double dist_factor[4], const double ox, const double oy,double *ix, double *iy ); + * + * @param ox + * @param oy + * @param ix + * @param iy + * @return + */ public int observ2Ideal(double ox,double oy,DoubleValue ix,DoubleValue iy) { double z02, z0, p, q, z, px, py,opttmp_1; + final double d0,d1,d3; + final double df[]=this.dist_factor; + d0=df[0]; + d1=df[1]; - px = ox - dist_factor[0]; - py = oy - dist_factor[1]; - p = dist_factor[2]/100000000.0; + px = ox - d0; + py = oy - d1; + p = df[2]/100000000.0; z02 = px*px+py*py; q = z0 = Math.sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py); @@ -267,10 +299,8 @@ public class NyARParam{ //Optimize opttmp_1 opttmp_1=p*z02; z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1); - //Optimize opttmp_1 - opttmp_1=z / z0; - px = px * opttmp_1; - py = py * opttmp_1; + px = px*z/z0; + py = py*z/z0; }else { px = 0.0; py = 0.0; @@ -282,12 +312,65 @@ public class NyARParam{ z02 = px*px+ py*py; z0 = Math.sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py); } - - ix.value=px / dist_factor[3] + dist_factor[0]; - iy.value=py / dist_factor[3] + dist_factor[1]; + d3=df[3]; + ix.value=px / d3 + d0; + iy.value=py / d3 + d1; return 0; } /** + * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。 + * @param i_x_coord + * @param i_y_coord + * @param i_start + * 開始点 + * @param i_num + * 計算数 + * @param o_ideal + * 出力バッファ[i_num][2]であること。 + */ + public void observ2IdealBatch(int[] i_x_coord,int[] i_y_coord,int i_start,int i_num,double[][] o_ideal) + { + double z02, z0,q, z, px, py,opttmp_1; + final double df[]=this.dist_factor; + final double d0=df[0]; + final double d1=df[1]; + final double d3=df[3]; + final double p = df[2]/100000000.0; + for(int j = 0; j < i_num; j++ ){ + + px = i_x_coord[i_start+j] - d0; + py = i_y_coord[i_start+j] - d1; + + z02 = px*px+py*py; + q = z0 = Math.sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py); + + for(int i = 1; ; i++ ) { + if( z0 != 0.0 ) { + //Optimize opttmp_1 + opttmp_1=p*z02; + z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1); + px = px*z/z0; + py = py*z/z0; + }else { + px = 0.0; + py = 0.0; + break; + } + if( i == PD_LOOP ){ + break; + } + z02 = px*px+ py*py; + z0 = Math.sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py); + } + o_ideal[j][0]=px / d3 + d0; + o_ideal[j][1]=py / d3 + d1; + } + } + + + + + /** * int arParamLoad( const char *filename, int num, ARParam *param, ...); * i_streamの入力ストリームからi_num個の設定を読み込み、パラメタを配列にして返します。 * @param filename @@ -318,7 +401,7 @@ public class NyARParam{ new_param.ysize=bb.getInt(); for(int i2=0;i2<3;i2++){ for(int i3=0;i3<4;i3++){ - new_param.mat[i2][i3]=bb.getDouble(); + new_param.array34[i2*4+i3]=bb.getDouble(); } } for(int i2=0;i2<4;i2++){ @@ -345,7 +428,7 @@ public class NyARParam{ bb.putInt(param[i].ysize); for(int i2=0;i2<3;i2++){ for(int i3=0;i3<4;i3++){ - bb.putDouble(param[i].mat[i2][i3]); + bb.putDouble(param[i].array34[i2*4+i3]); } } for(int i2=0;i2<4;i2++){ diff --git a/src/jp/nyatla/nyartoolkit/core/NyARSquare.java b/src/jp/nyatla/nyartoolkit/core/NyARSquare.java index 187f9db..8ffb0f0 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARSquare.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARSquare.java @@ -30,28 +30,93 @@ * */ package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.util.DoubleValue; + /** * ARMarkerInfoに相当するクラス。 * スクエア情報を保持します。 * */ -public class NyARSquare{ - private NyARMarker marker; - public int area; - public double[] pos; - public double[][] line; //double[4][3] - public double[][] vertex;//double[4][2]; - public NyARSquare(NyARMarker i_marker,double[][] i_attached_line,double[][] i_attached_vertex) +public class NyARSquare extends NyARMarker{ +// private NyARMarker marker; +// public int area; +// public double[] pos; + public double[][] line=new double[4][3]; //double[4][3] + public double[][] sqvertex=new double[4][2];//double[4][2]; + public NyARSquare() { - //ARSquareは、ARMarkerを完全にラップするようにした。 - marker=i_marker; - area=i_marker.area; - pos =i_marker.pos; - line =i_attached_line; - vertex=i_attached_vertex; + super(); } - public NyARMarker getMarker() + private final NyARMat wk_getLine_input=new NyARMat(1,2); + private final NyARMat wk_getLine_evec=new NyARMat(2,2); + private final NyARVec wk_getLine_ev=new NyARVec(2); + private final NyARVec wk_getLine_mean=new NyARVec(2); + /** + * arGetLine(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2]) + * arGetLine2(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2], double *dist_factor) + * の２関数の合成品です。 + * 格納しているマーカー情報に対して、GetLineの計算を行い、結果を返します。 + * Optimize:STEP[424->391] + * @param i_cparam + * @return + * @throws NyARException + */ + public boolean getLine(NyARParam i_cparam) throws NyARException { - return marker; + double w1; + int st, ed, n; + int i; + + final double[][] l_sqvertex=this.sqvertex; + final double[][] l_line=this.line; + final int[] l_mkvertex=this.mkvertex; + final int[] l_x_coord=this.x_coord; + final int[] l_y_coord=this.y_coord; + final NyARVec ev = this.wk_getLine_ev; //matrixPCAの戻り値を受け取る + final NyARVec mean = this.wk_getLine_mean;//matrixPCAの戻り値を受け取る + final double[] mean_array=mean.getArray(); + double[] l_line_i,l_line_2; + + NyARMat input=this.wk_getLine_input;//次処理で初期化される。 + NyARMat evec =this.wk_getLine_evec;//アウトパラメータを受け取るから初期化不要//new NyARMat(2,2); + double[][] evec_array=evec.getArray(); + for( i = 0; i < 4; i++ ) { + w1 = (double)(l_mkvertex[i+1]-l_mkvertex[i]+1) * 0.05 + 0.5; + st = (int)(l_mkvertex[i] + w1); + ed = (int)(l_mkvertex[i+1] - w1); + n = ed - st + 1; + if(n<2){ + //nが2以下でmatrix.PCAを計算することはできないので、エラーにしておく。 + return false;//throw new NyARException(); + } + input.realloc(n,2); + //バッチ取得 + i_cparam.observ2IdealBatch(l_x_coord,l_y_coord,st,n,input.getArray()); +// for( j = 0; j < n; j++ ) { +// i_cparam.observ2Ideal(l_x_coord[st+j], l_y_coord[st+j],dv1,dv2);//arParamObserv2Ideal( dist_factor, x_coord[st+j], y_coord[st+j],&(input->m[j*2+0]), &(input->m[j*2+1]) ); +// in_array[j][0]=dv1.value; +// in_array[j][1]=dv2.value; +// } + input.matrixPCA(evec, ev, mean); + l_line_i=l_line[i]; + l_line_i[0] = evec_array[0][1];//line[i][0] = evec->m[1]; + l_line_i[1] = -evec_array[0][0];//line[i][1] = -evec->m[0]; + l_line_i[2] = -(l_line_i[0]*mean_array[0] + l_line_i[1]*mean_array[1]);//line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]); + } + + for( i = 0; i < 4; i++ ) + { + l_line_i=l_line[i]; + l_line_2=l_line[(i+3)%4]; + w1 = l_line_2[0] * l_line_i[1] - l_line_i[0] * l_line_2[1]; + if( w1 == 0.0 ){ + return false; + } + l_sqvertex[i][0] = ( l_line_2[1] * l_line_i[2]- l_line_i[1] * l_line_2[2] ) / w1; + l_sqvertex[i][1] = ( l_line_i[0] * l_line_2[2]- l_line_2[0] * l_line_i[2] ) / w1; + } + return true; } } diff --git a/src/jp/nyatla/nyartoolkit/core/NyARSquareList.java b/src/jp/nyatla/nyartoolkit/core/NyARSquareList.java new file mode 100644 index 0000000..638c302 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARSquareList.java @@ -0,0 +1,70 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +public class NyARSquareList extends NyARMarkerList +{ + private final NyARSquare[] square_array; + private int square_array_num; + public NyARSquareList(int i_number_of_holder) + { + super(new NyARSquare[i_number_of_holder]); + //マーカーホルダに実体を割り当てる。 + for(int i=0;i=this.square_array_num){ + throw new NyARException(); + } + return this.square_array[i_index]; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARTransMat.java b/src/jp/nyatla/nyartoolkit/core/NyARTransMat.java index fe7013a..3e072a7 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARTransMat.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARTransMat.java @@ -43,25 +43,23 @@ import jp.nyatla.util.DoubleValue; * */ public class NyARTransMat{ - private static final int AR_FITTING_TO_IDEAL=0;//#define AR_FITTING_TO_IDEAL 0 - private static final int AR_FITTING_TO_INPUT=1;//#define AR_FITTING_TO_INPUT 1 - private static final int arFittingMode =AR_FITTING_TO_INPUT; + private final static int AR_FITTING_TO_IDEAL=0;//#define AR_FITTING_TO_IDEAL 0 + private final static int AR_FITTING_TO_INPUT=1;//#define AR_FITTING_TO_INPUT 1 + private final static int arFittingMode =AR_FITTING_TO_INPUT; - private static final int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5 - private static final double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0 - private final static int P_MAX=500;//#define P_MAX 500 - private double[][] pos3d=new double[P_MAX][3];//pos3d[P_MAX][3]; - private double[][] pos2d=new double[P_MAX][2];//pos2d[P_MAX][2]; - - private double[] center={0.0,0.0}; - private NyARParam param; - private double conv[][]; - private NyARMat result_mat=new NyARMat(3,4); - public NyARTransMat(NyARParam i_param) + private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5 + private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0 + private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX 500 + private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数 + private final NyARTransRot transrot; + private final double[] center={0.0,0.0}; + private final NyARParam param; + private final NyARMat result_mat=new NyARMat(3,4); + public NyARTransMat(NyARParam i_param)throws NyARException { - conv=result_mat.getArray(); param=i_param; - + transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX); + } public void setCenter(double i_x,double i_y) { @@ -72,6 +70,11 @@ public class NyARTransMat{ { return result_mat; } + + private final double[] wk_transMat_pos3d=new double[P_MAX*3];//pos3d[P_MAX][3]; + private final double[][] wk_transMat_ppos2d=new double[4][2]; + private final double[][] wk_transMat_ppos3d=new double[4][2]; + private final double[] wk_transMat_off=new double[3]; /** * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] ) * 関数の置き換え。 @@ -85,796 +88,252 @@ public class NyARTransMat{ */ public double transMat( NyARSquare square,int i_direction, double width)throws NyARException { - double[][] rot=new double[3][3]; - double[][] ppos2d=new double[4][2]; - double[][] ppos3d=new double[4][2]; - int dir; - double err=-1; + double[][] ppos2d=wk_transMat_ppos2d; + double[][] ppos3d=wk_transMat_ppos3d; + double[] off=wk_transMat_off; + double[] pos3d=wk_transMat_pos3d; + int dir; + double err=-1; - - if( arGetInitRot( square,i_direction, rot ) < 0 ){ - throw new NyARException();//return -1; - } - - dir = i_direction; - ppos2d[0][0] = square.vertex[(4-dir)%4][0]; - ppos2d[0][1] = square.vertex[(4-dir)%4][1]; - ppos2d[1][0] = square.vertex[(5-dir)%4][0]; - ppos2d[1][1] = square.vertex[(5-dir)%4][1]; - ppos2d[2][0] = square.vertex[(6-dir)%4][0]; - ppos2d[2][1] = square.vertex[(6-dir)%4][1]; - ppos2d[3][0] = square.vertex[(7-dir)%4][0]; - ppos2d[3][1] = square.vertex[(7-dir)%4][1]; - ppos3d[0][0] = center[0] - width/2.0; - ppos3d[0][1] = center[1] + width/2.0; - ppos3d[1][0] = center[0] + width/2.0; - ppos3d[1][1] = center[1] + width/2.0; - ppos3d[2][0] = center[0] + width/2.0; - ppos3d[2][1] = center[1] - width/2.0; - ppos3d[3][0] = center[0] - width/2.0; - ppos3d[3][1] = center[1] - width/2.0; - + transrot.initRot(square,i_direction); - - - - for(int i=0;i pmax[0] ){ - pmax[0] = ppos3d[i][0]; - } - if( ppos3d[i][0] < pmin[0] ){ - pmin[0] = ppos3d[i][0]; - } - if( ppos3d[i][1] > pmax[1] ){ - pmax[1] = ppos3d[i][1]; - } - if( ppos3d[i][1] < pmin[1] ){ - pmin[1] = ppos3d[i][1]; - } + double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3]; + double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double off[3], pmax[3], pmin[3]; + pmax[0]=pmax[1]=pmax[2] = -10000000000.0; + pmin[0]=pmin[1]=pmin[2] = 10000000000.0; + for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) { + if( i_ppos3d[i][0] > pmax[0] ){ + pmax[0] = i_ppos3d[i][0]; + } + if( i_ppos3d[i][0] < pmin[0] ){ + pmin[0] = i_ppos3d[i][0]; + } + if( i_ppos3d[i][1] > pmax[1] ){ + pmax[1] = i_ppos3d[i][1]; + } + if( i_ppos3d[i][1] < pmin[1] ){ + pmin[1] = i_ppos3d[i][1]; + } } - off[0] = -(pmax[0] + pmin[0]) / 2.0; - off[1] = -(pmax[1] + pmin[1]) / 2.0; - off[2] = -(pmax[2] + pmin[2]) / 2.0; - for(int i = 0; i < num; i++ ) { - pos3d[i][0] = ppos3d[i][0] + off[0]; - pos3d[i][1] = ppos3d[i][1] + off[1]; - - pos3d[i][2] = 0.0; - } - ret = arGetTransMatSub( rot, ppos2d, pos3d, num); - - conv[0][3] = conv[0][0]*off[0] + conv[0][1]*off[1] + conv[0][2]*off[2] + conv[0][3]; - conv[1][3] = conv[1][0]*off[0] + conv[1][1]*off[1] + conv[1][2]*off[2] + conv[1][3]; - conv[2][3] = conv[2][0]*off[0] + conv[2][1]*off[1] + conv[2][2]*off[2] + conv[2][3]; - - return ret; + o_off[0] = -(pmax[0] + pmin[0]) / 2.0; + o_off[1] = -(pmax[1] + pmin[1]) / 2.0; + o_off[2] = -(pmax[2] + pmin[2]) / 2.0; + for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) { + o_pos3d[i*3+0] = i_ppos3d[i][0] + o_off[0]; + o_pos3d[i*3+1] = i_ppos3d[i][1] + o_off[1]; + o_pos3d[i*3+2] = 0.0; + } + } + + + /** + * double arGetTransMat3( double rot[3][3], double ppos2d[][2],double ppos3d[][2], int num, double conv[3][4],double *dist_factor, double cpara[3][4] ) + * STEP 414-> + * @param ppos2d + * @param i_pos3d + * @param i_off + * @param num + * @return + * @throws NyARException + */ + private final double arGetTransMat3( + double ppos2d[][], + final double i_pos3d[], + final double i_off[])throws NyARException{ + + double ret; + ret = arGetTransMatSub(ppos2d, i_pos3d); + double[][] conv=result_mat.getArray(); + conv[0][3] = conv[0][0]*i_off[0] + conv[0][1]*i_off[1] + conv[0][2]*i_off[2] + conv[0][3]; + conv[1][3] = conv[1][0]*i_off[0] + conv[1][1]*i_off[1] + conv[1][2]*i_off[2] + conv[1][3]; + conv[2][3] = conv[2][0]*i_off[0] + conv[2][1]*i_off[1] + conv[2][2]*i_off[2] + conv[2][3]; + return ret; } - /*static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )*/ - private double arGetTransMatSub( double rot[][], double ppos2d[][],double pos3d[][], int num) throws NyARException + private final NyARMat wk_arGetTransMatSub_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3); + private final NyARMat wk_arGetTransMatSub_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2); + private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1); + private final NyARMat wk_arGetTransMatSub_mat_d=new NyARMat( 3, 3 ); + private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 ); + private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 ); + private final double[] wk_arGetTransMatSub_trans=new double[3]; + private final double[] wk_arGetTransMatSub_pos2d=new double[P_MAX*2];//pos2d[P_MAX][2]; + private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue(); + private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue(); + + + /** + * static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] ) + * Optimize:2008.04.20:STEP[1033→1004] + * @param rot + * @param ppos2d + * @param pos3d + * @param num + * @return + * @throws NyARException + */ + private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[]) throws NyARException { - double cpara[][]=param.getMat(); + double[] pos2d=wk_arGetTransMatSub_pos2d; + double cpara[]=param.get34Array(); NyARMat mat_a,mat_b,mat_c,mat_d,mat_e,mat_f;//ARMat *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f; - double[] trans=new double[3];//double trans[3]; + double wx, wy, wz; double ret; int i, j; - mat_a = new NyARMat(num*2, 3 ); - mat_b = new NyARMat( 3, num*2 ); - mat_c = new NyARMat( num*2, 1 ); - mat_d = new NyARMat( 3, 3 ); - mat_e = new NyARMat( 3, 1 ); - mat_f = new NyARMat( 3, 1 ); + + mat_a =this.wk_arGetTransMatSub_mat_a; +// mat_a.realloc(NUMBER_OF_VERTEX*2,3); double[][] a_array=mat_a.getArray(); + + mat_b =this.wk_arGetTransMatSub_mat_b; +// mat_b.realloc(3,NUMBER_OF_VERTEX*2); double[][] b_array=mat_b.getArray(); - double[][] c_array=mat_c.getArray(); - double[][] f_array=mat_f.getArray(); - + + + DoubleValue a1=wk_arGetTransMatSub_a1; + DoubleValue a2=wk_arGetTransMatSub_a2; if(arFittingMode == AR_FITTING_TO_INPUT ){ - DoubleValue a1=new DoubleValue(),a2=new DoubleValue(); - for( i = 0; i < num; i++ ) { - param.ideal2Observ(ppos2d[i][0], ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]); - pos2d[i][0]=a1.value; - pos2d[i][1]=a2.value; + for( i = 0; i < NUMBER_OF_VERTEX; i++ ) { + param.ideal2Observ(i_ppos2d[i][0], i_ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]); + pos2d[i*2+0]=a1.value; + pos2d[i*2+1]=a2.value; } }else{ - for( i = 0; i < num; i++ ) { - pos2d[i][0] = ppos2d[i][0]; - pos2d[i][1] = ppos2d[i][1]; + for( i = 0; i < NUMBER_OF_VERTEX; i++ ) { + pos2d[i*2+0] = i_ppos2d[i][0]; + pos2d[i*2+1] = i_ppos2d[i][1]; } } - for( j = 0; j < num; j++ ) { + mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。 +// mat_c.realloc(NUMBER_OF_VERTEX*2,1); + double[][] c_array=mat_c.getArray(); + double[] rot=transrot.getArray(); + for( j = 0; j < NUMBER_OF_VERTEX; j++ ) { int x2=j*2; - wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2]; - wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2]; - wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2]; - a_array[x2 ][0]=b_array[0][x2]=cpara[0][0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0]; - a_array[x2 ][1]=b_array[1][x2]=cpara[0][1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1]; - a_array[x2 ][2]=b_array[2][x2]=cpara[0][2] - pos2d[j][0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0]; + // + //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2]; + //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2]; + //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2]; + wx = rot[0] * i_pos3d[j*3+0]+ rot[1] * i_pos3d[j*3+1]+ rot[2] * i_pos3d[j*3+2]; + wy = rot[3] * i_pos3d[j*3+0]+ rot[4] * i_pos3d[j*3+1]+ rot[5] * i_pos3d[j*3+2]; + wz = rot[6] * i_pos3d[j*3+0]+ rot[7] * i_pos3d[j*3+1]+ rot[8] * i_pos3d[j*3+2]; + // + a_array[x2 ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0]; + a_array[x2 ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1]; + a_array[x2 ][2]=b_array[2][x2]=cpara[0*4+2] - pos2d[j*2+0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0]; a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0; - a_array[x2+1][1]=b_array[1][x2+1]=cpara[1][1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1]; - a_array[x2+1][2]=b_array[2][x2+1]=cpara[1][2] - pos2d[j][1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1]; - c_array[x2][0] =wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz; - c_array[x2+1][0]=wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz; + a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1]; + a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[j*2+1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1]; + c_array[x2][0] =wz * pos2d[j*2+0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz; + c_array[x2+1][0]=wz * pos2d[j*2+1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz; } -// JartkException.trap("未チェックのパス");{ - NyARMat.matrixMul( mat_d, mat_b, mat_a ); - NyARMat.matrixMul( mat_e, mat_b, mat_c ); + mat_d = this.wk_arGetTransMatSub_mat_d;//次処理で値をもらうので、初期化の必要は無い。 + mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。 + mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。 + double[][] f_array=mat_f.getArray(); + + mat_d.matrixMul(mat_b, mat_a ); + mat_e.matrixMul(mat_b, mat_c ); mat_d.matrixSelfInv(); - NyARMat.matrixMul( mat_f, mat_d, mat_e ); -// } + mat_f.matrixMul(mat_d, mat_e ); + + double[] trans=wk_arGetTransMatSub_trans;//double trans[3]; trans[0] = f_array[0][0];//trans[0] = mat_f->m[0]; trans[1] = f_array[1][0]; trans[2] = f_array[2][0];//trans[2] = mat_f->m[2]; - ret = arModifyMatrix( rot, trans, pos3d, pos2d, num ); - for( j = 0; j < num; j++ ) { + ret =transrot.modifyMatrix(trans, i_pos3d, pos2d); + for( j = 0; j < NUMBER_OF_VERTEX; j++ ) { int x2=j*2; - wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2]; - wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2]; - wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2]; - a_array[x2 ][0]=b_array[0][x2]=cpara[0][0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0]; - a_array[x2 ][1]=b_array[1][x2]=cpara[0][1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1]; - a_array[x2 ][2]=b_array[2][x2]=cpara[0][2] - pos2d[j][0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0]; + // + //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2]; + //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2]; + //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2]; + wx = rot[0] * i_pos3d[j*3+0]+ rot[1] * i_pos3d[j*3+1]+ rot[2] * i_pos3d[j*3+2]; + wy = rot[3] * i_pos3d[j*3+0]+ rot[4] * i_pos3d[j*3+1]+ rot[5] * i_pos3d[j*3+2]; + wz = rot[6] * i_pos3d[j*3+0]+ rot[7] * i_pos3d[j*3+1]+ rot[8] * i_pos3d[j*3+2]; + // + a_array[x2 ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0]; + a_array[x2 ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1]; + a_array[x2 ][2]=b_array[2][x2]=cpara[0*4+2] - pos2d[j*2+0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0]; a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0; - a_array[x2+1][1]=b_array[1][x2+1]=cpara[1][1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1]; - a_array[x2+1][2]=b_array[2][x2+1]=cpara[1][2] - pos2d[j][1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1]; - c_array[x2][0] =wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz; - c_array[x2+1][0]=wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz; + a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1]; + a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[j*2+1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1]; + c_array[x2][0] =wz * pos2d[j*2+0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz; + c_array[x2+1][0]=wz * pos2d[j*2+1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz; } // JartkException.trap("未チェックのパス");{ - NyARMat.matrixMul( mat_d, mat_b, mat_a ); - NyARMat.matrixMul( mat_e, mat_b, mat_c ); + mat_d.matrixMul(mat_b, mat_a ); + mat_e.matrixMul(mat_b, mat_c ); mat_d.matrixSelfInv(); - NyARMat.matrixMul( mat_f, mat_d, mat_e ); + mat_f.matrixMul(mat_d, mat_e ); // } trans[0] = f_array[0][0];//trans[0] = mat_f->m[0]; trans[1] = f_array[1][0]; trans[2] = f_array[2][0];//trans[2] = mat_f->m[2]; - ret = arModifyMatrix( rot, trans, pos3d, pos2d, num ); - for( j = 0; j < 3; j++ ) { - for( i = 0; i < 3; i++ ){ - conv[j][i] = rot[j][i]; - } + ret = transrot.modifyMatrix(trans, i_pos3d, pos2d); + double[][] conv=result_mat.getArray(); + for( j = 2; j >=0; j-- ) {//for( j = 0; j < 3; j++ ) { + // + //for( i = 0; i < 3; i++ ){ + // conv[j][i] = rot[j][i]; + //} + conv[j][0] = rot[j*3+0]; + conv[j][1] = rot[j*3+1]; + conv[j][2] = rot[j*3+2]; + // conv[j][3] = trans[j]; } return ret; } - private double arModifyMatrix( double rot[][], double trans[],double vertex[][], double pos2d[][], int num ) - { - double factor; - DoubleValue a=new DoubleValue(),b=new DoubleValue(),c=new DoubleValue();//double a, b, c; - double a1, b1, c1; - double a2, b2, c2; - double ma = 0.0, mb = 0.0, mc = 0.0; - double[][] combo=new double[3][4]; - double hx, hy, h, x, y; - double err, minerr=0; - int t1, t2, t3; - int s1 = 0, s2 = 0, s3 = 0; - int i, j; - - arGetAngle( rot, a, b, c);//arGetAngle( rot, &a, &b, &c ); - a2 = a.value; - b2 = b.value; - c2 = c.value; - factor = 10.0*Math.PI/180.0; - for( j = 0; j < 10; j++ ) { - minerr = 1000000000.0; - for(t1=-1;t1<=1;t1++) { - for(t2=-1;t2<=1;t2++) { - for(t3=-1;t3<=1;t3++) { - a1 = a2 + factor*t1; - b1 = b2 + factor*t2; - c1 = c2 + factor*t3; - arGetNewMatrix( a1, b1, c1, trans, null, combo ); - err = 0.0; - for( i = 0; i < num; i++ ) { - hx = combo[0][0] * vertex[i][0]+ combo[0][1] * vertex[i][1]+ combo[0][2] * vertex[i][2]+ combo[0][3]; - hy = combo[1][0] * vertex[i][0]+ combo[1][1] * vertex[i][1]+ combo[1][2] * vertex[i][2]+ combo[1][3]; - h = combo[2][0] * vertex[i][0]+ combo[2][1] * vertex[i][1]+ combo[2][2] * vertex[i][2]+ combo[2][3]; - x = hx / h; - y = hy / h; - err += (pos2d[i][0] - x) * (pos2d[i][0] - x)+ (pos2d[i][1] - y) * (pos2d[i][1] - y); - } - if( err < minerr ) { - minerr = err; - ma = a1; - mb = b1; - mc = c1; - s1 = t1; - s2 = t2; - s3 = t3; - } - } - } - } - if( s1 == 0 && s2 == 0 && s3 == 0 ){ - factor *= 0.5; - } - a2 = ma; - b2 = mb; - c2 = mc; - } - arGetRot( ma, mb, mc, rot ); - /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ - return minerr/num; - } - private double[][] wk_cpara2_arGetNewMatrix=new double[3][4]; - private double[][] wk_rot_arGetNewMatrix =new double[3][3]; - /** - * - * @param a - * @param b - * @param c - * @param trans - * @param trans2 - * @param ret - * @return - */ - private int arGetNewMatrix( double a, double b, double c,double trans[], double trans2[][], double ret[][] ) - { - double cpara[][]=param.getMat(); - double[][] cpara2=wk_cpara2_arGetNewMatrix; //この関数で初期化される。 - double[][] rot =wk_rot_arGetNewMatrix; //arGetRotで初期化される。 - int i, j; - - arGetRot( a, b, c, rot ); - - if( trans2 != null ) { - for( j = 0; j < 3; j++ ) { -// for( i = 0; i < 4; i++ ) { -// cpara2[j][i] = cpara[j][0] * trans2[0][i]+ cpara[j][1] * trans2[1][i]+ cpara[j][2] * trans2[2][i]; -// } -//Optimize - cpara2[j][0] = cpara[j][0] * trans2[0][0]+ cpara[j][1] * trans2[1][0]+ cpara[j][2] * trans2[2][0]; - cpara2[j][1] = cpara[j][0] * trans2[0][1]+ cpara[j][1] * trans2[1][1]+ cpara[j][2] * trans2[2][1]; - cpara2[j][2] = cpara[j][0] * trans2[0][2]+ cpara[j][1] * trans2[1][2]+ cpara[j][2] * trans2[2][2]; - cpara2[j][3] = cpara[j][0] * trans2[0][3]+ cpara[j][1] * trans2[1][3]+ cpara[j][2] * trans2[2][3]; - } - }else{ - for( j = 0; j < 3; j++ ) { -// for( i = 0; i < 4; i++ ) { -// cpara2[j][i] = cpara[j][i]; -// } -//Optimize - cpara2[j][0] = cpara[j][0]; - cpara2[j][1] = cpara[j][1]; - cpara2[j][2] = cpara[j][2]; - cpara2[j][3] = cpara[j][3]; - } - } - for( j = 0; j < 3; j++ ) { -// for( i = 0; i < 3; i++ ) { -// ret[j][i] = cpara2[j][0] * rot[0][i]+ cpara2[j][1] * rot[1][i]+ cpara2[j][2] * rot[2][i]; -// } -//Optimize - ret[j][0] = cpara2[j][0] * rot[0][0]+ cpara2[j][1] * rot[1][0]+ cpara2[j][2] * rot[2][0]; - ret[j][1] = cpara2[j][0] * rot[0][1]+ cpara2[j][1] * rot[1][1]+ cpara2[j][2] * rot[2][1]; - ret[j][2] = cpara2[j][0] * rot[0][2]+ cpara2[j][1] * rot[1][2]+ cpara2[j][2] * rot[2][2]; -// - ret[j][3] = cpara2[j][0] * trans[0]+ cpara2[j][1] * trans[1]+ cpara2[j][2] * trans[2]+ cpara2[j][3]; - } - return(0); - } - private void arGetRot( double a, double b, double c, double rot[][] ) - { - double sina, sinb, sinc; - double cosa, cosb, cosc; - - sina = Math.sin(a); - cosa = Math.cos(a); - sinb = Math.sin(b); - cosb = Math.cos(b); - sinc = Math.sin(c); - cosc = Math.cos(c); - //Optimize - double A,B,C,D,E,F,G; - A=sina*cosa; - B=sina*sina; - C=cosa*cosa; - D=cosa*sinb; - F=cosb*cosc; - E=cosb*sinc; - G=sina*sinb; - - rot[0][0] = C*F + B*cosc + A*(E - sinc); - rot[0][1] = -C*E-B*sinc+A*(F-cosc); - rot[0][2] = D; - rot[1][0] = A*(F-cosc)+B*E+C*sinc; - rot[1][1] = A*(-E+sinc)+B*F+C*cosc; - rot[1][2] = G; - rot[2][0] = -D*cosc-G*sinc; - rot[2][1] = D*sinc-G*cosc; - rot[2][2] = cosb; - /* - //49回 - rot[0][0] = cosa*cosa*cosb*cosc+sina*sina*cosc+sina*cosa*cosb*sinc-sina*cosa*sinc; - rot[0][1] = -cosa*cosa*cosb*sinc-sina*sina*sinc+sina*cosa*cosb*cosc-sina*cosa*cosc; - rot[0][2] = cosa*sinb; - rot[1][0] = sina*cosa*cosb*cosc-sina*cosa*cosc+sina*sina*cosb*sinc+cosa*cosa*sinc; - rot[1][1] = -sina*cosa*cosb*sinc+sina*cosa*sinc+sina*sina*cosb*cosc+cosa*cosa*cosc; - rot[1][2] = sina*sinb; - rot[2][0] = -cosa*sinb*cosc-sina*sinb*sinc; - rot[2][1] = cosa*sinb*sinc-sina*sinb*cosc; - rot[2][2] = cosb; -*/ - } - /*int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )*/ - private int arGetAngle( double rot[][], DoubleValue wa, DoubleValue wb, DoubleValue wc ) - { - double a, b, c; - double sina, cosa, sinb, cosb, sinc, cosc; -// #if CHECK_CALC -// double w[3]; -// int i; -// for(i=0;i<3;i++) w[i] = rot[i][0]; -// for(i=0;i<3;i++) rot[i][0] = rot[i][1]; -// for(i=0;i<3;i++) rot[i][1] = rot[i][2]; -// for(i=0;i<3;i++) rot[i][2] = w[i]; -// #endif - if( rot[2][2] > 1.0 ) { - /* printf("cos(beta) = %f\n", rot[2][2]); */ - rot[2][2] = 1.0; - }else if( rot[2][2] < -1.0 ) { - /* printf("cos(beta) = %f\n", rot[2][2]); */ - rot[2][2] = -1.0; - } - cosb = rot[2][2]; - b = Math.acos( cosb ); - sinb = Math.sin( b ); - if( b >= 0.000001 || b <= -0.000001) { - cosa = rot[0][2] / sinb; - sina = rot[1][2] / sinb; - if( cosa > 1.0 ) { - /* printf("cos(alph) = %f\n", cosa); */ - cosa = 1.0; - sina = 0.0; - } - if( cosa < -1.0 ) { - /* printf("cos(alph) = %f\n", cosa); */ - cosa = -1.0; - sina = 0.0; - } - if( sina > 1.0 ) { - /* printf("sin(alph) = %f\n", sina); */ - sina = 1.0; - cosa = 0.0; - } - if( sina < -1.0 ) { - /* printf("sin(alph) = %f\n", sina); */ - sina = -1.0; - cosa = 0.0; - } - a = Math.acos( cosa ); - if( sina < 0 ){ - a = -a; - } - sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]); - cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]); - if( cosc > 1.0 ) { - /* printf("cos(r) = %f\n", cosc); */ - cosc = 1.0; - sinc = 0.0; - } - if( cosc < -1.0 ) { - /* printf("cos(r) = %f\n", cosc); */ - cosc = -1.0; - sinc = 0.0; - } - if( sinc > 1.0 ) { - /* printf("sin(r) = %f\n", sinc); */ - sinc = 1.0; - cosc = 0.0; - } - if( sinc < -1.0 ) { - /* printf("sin(r) = %f\n", sinc); */ - sinc = -1.0; - cosc = 0.0; - } - c = Math.acos( cosc ); - if( sinc < 0 ){ - c = -c; - } - }else { - a = b = 0.0; - cosa = cosb = 1.0; - sina = sinb = 0.0; - cosc = rot[0][0]; - sinc = rot[1][0]; - if( cosc > 1.0 ) { - /* printf("cos(r) = %f\n", cosc); */ - cosc = 1.0; - sinc = 0.0; - } - if( cosc < -1.0 ) { - /* printf("cos(r) = %f\n", cosc); */ - cosc = -1.0; - sinc = 0.0; - } - if( sinc > 1.0 ) { - /* printf("sin(r) = %f\n", sinc); */ - sinc = 1.0; - cosc = 0.0; - } - if( sinc < -1.0 ) { - /* printf("sin(r) = %f\n", sinc); */ - sinc = -1.0; - cosc = 0.0; - } - c = Math.acos( cosc ); - if( sinc < 0 ) c = -c; - } - - wa.value=a;//*wa = a; - wb.value=b;//*wb = b; - wc.value=c;//*wc = c; - - return 0; - } - /*int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] )*/ - private int arGetInitRot( NyARSquare marker_info,int i_direction, double rot[][] ) throws NyARException - { - double cpara[][]=param.getMat(); - double[][] wdir=new double[3][3]; - double w, w1, w2, w3; - int dir; - int j; - - dir = i_direction; - - for( j = 0; j < 2; j++ ) { - w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1]; - w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2]; - w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0]; - - wdir[j][0] = w1*(cpara[0][1]*cpara[1][2]-cpara[0][2]*cpara[1][1])+ w2*cpara[1][1]- w3*cpara[0][1]; - wdir[j][1] = -w1*cpara[0][0]*cpara[1][2]+ w3*cpara[0][0]; - wdir[j][2] = w1*cpara[0][0]*cpara[1][1]; - w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] ); - wdir[j][0] /= w; - wdir[j][1] /= w; - wdir[j][2] /= w; - } - - if( check_dir(wdir[0], marker_info.vertex[(4-dir)%4],marker_info.vertex[(5-dir)%4], cpara) < 0 ){ - return -1; - } - if( check_dir(wdir[1], marker_info.vertex[(7-dir)%4],marker_info.vertex[(4-dir)%4], cpara) < 0 ){ - return -1; - } - if( check_rotation(wdir) < 0 ){ - return -1; - } - - wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1]; - wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2]; - wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0]; - w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] ); - wdir[2][0] /= w; - wdir[2][1] /= w; - wdir[2][2] /= w; - /* - if( wdir[2][2] < 0 ) { - wdir[2][0] /= -w; - wdir[2][1] /= -w; - wdir[2][2] /= -w; - } - else { - wdir[2][0] /= w; - wdir[2][1] /= w; - wdir[2][2] /= w; - } - */ - - rot[0][0] = wdir[0][0]; - rot[1][0] = wdir[0][1]; - rot[2][0] = wdir[0][2]; - rot[0][1] = wdir[1][0]; - rot[1][1] = wdir[1][1]; - rot[2][1] = wdir[1][2]; - rot[0][2] = wdir[2][0]; - rot[1][2] = wdir[2][1]; - rot[2][2] = wdir[2][2]; - - return 0; - } - - /*static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] )*/ - private static int check_dir( double dir[], double st[], double ed[],double cpara[][]) throws NyARException - { - - double[][] world=new double[2][3]; - double[][] camera=new double[2][2]; - double[][] v=new double[2][2]; - double h; - int i, j; - // JartkException.trap("未チェックパス"); - NyARMat mat_a = new NyARMat( 3, 3 ); - double[][] a_array=mat_a.getArray(); - for(j=0;j<3;j++){ - for(i=0;i<3;i++){ - a_array[j][i]=cpara[j][i];//m[j*3+i] = cpara[j][i]; - } - } - // JartkException.trap("未チェックのパス"); - mat_a.matrixSelfInv(); - world[0][0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ mat_a->m[2]*10.0; - world[0][1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ mat_a->m[5]*10.0; - world[0][2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ mat_a->m[8]*10.0; - world[1][0] = world[0][0] + dir[0]; - world[1][1] = world[0][1] + dir[1]; - world[1][2] = world[0][2] + dir[2]; - - for( i = 0; i < 2; i++ ) { - h = cpara[2][0] * world[i][0]+ cpara[2][1] * world[i][1]+ cpara[2][2] * world[i][2]; - if( h == 0.0 ){ - return -1; - } - camera[i][0] = (cpara[0][0] * world[i][0]+ cpara[0][1] * world[i][1]+ cpara[0][2] * world[i][2]) / h; - camera[i][1] = (cpara[1][0] * world[i][0]+ cpara[1][1] * world[i][1]+ cpara[1][2] * world[i][2]) / h; - } - - v[0][0] = ed[0] - st[0]; - v[0][1] = ed[1] - st[1]; - v[1][0] = camera[1][0] - camera[0][0]; - v[1][1] = camera[1][1] - camera[0][1]; - - if( v[0][0]*v[1][0] + v[0][1]*v[1][1] < 0 ) { - dir[0] = -dir[0]; - dir[1] = -dir[1]; - dir[2] = -dir[2]; - } - return 0; - } - - /*int check_rotation( double rot[2][3] )*/ - private static int check_rotation( double rot[][] ) - { - double[] v1=new double[3], v2=new double[3], v3=new double[3]; - double ca, cb, k1, k2, k3, k4; - double a, b, c, d; - double p1, q1, r1; - double p2, q2, r2; - double p3, q3, r3; - double p4, q4, r4; - double w; - double e1, e2, e3, e4; - int f; - - v1[0] = rot[0][0]; - v1[1] = rot[0][1]; - v1[2] = rot[0][2]; - v2[0] = rot[1][0]; - v2[1] = rot[1][1]; - v2[2] = rot[1][2]; - v3[0] = v1[1]*v2[2] - v1[2]*v2[1]; - v3[1] = v1[2]*v2[0] - v1[0]*v2[2]; - v3[2] = v1[0]*v2[1] - v1[1]*v2[0]; - w = Math.sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] ); - if( w == 0.0 ) return -1; - v3[0] /= w; - v3[1] /= w; - v3[2] /= w; - - cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; - if( cb < 0 ) cb *= -1.0; - ca = (Math.sqrt(cb+1.0) + Math.sqrt(1.0-cb)) * 0.5; - - if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) { - f = 0; - } - else { - if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) { - w = v1[1]; v1[1] = v1[2]; v1[2] = w; - w = v3[1]; v3[1] = v3[2]; v3[2] = w; - f = 1; - } - else { - w = v1[0]; v1[0] = v1[2]; v1[2] = w; - w = v3[0]; v3[0] = v3[2]; v3[2] = w; - f = 2; - } - } - if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ){ - return -1; - } - k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]); - k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]); - k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]); - k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]); - - a = k1*k1 + k3*k3 + 1; - b = k1*k2 + k3*k4; - c = k2*k2 + k4*k4 - 1; - - d = b*b - a*c; - if( d < 0 ){ - return -1; - } - r1 = (-b + Math.sqrt(d))/a; - p1 = k1*r1 + k2; - q1 = k3*r1 + k4; - r2 = (-b - Math.sqrt(d))/a; - p2 = k1*r2 + k2; - q2 = k3*r2 + k4; - if( f == 1 ) { - w = q1; q1 = r1; r1 = w; - w = q2; q2 = r2; r2 = w; - w = v1[1]; v1[1] = v1[2]; v1[2] = w; - w = v3[1]; v3[1] = v3[2]; v3[2] = w; - f = 0; - } - if( f == 2 ) { - w = p1; p1 = r1; r1 = w; - w = p2; p2 = r2; r2 = w; - w = v1[0]; v1[0] = v1[2]; v1[2] = w; - w = v3[0]; v3[0] = v3[2]; v3[2] = w; - f = 0; - } - - if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) { - f = 0; - } - else { - if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) { - w = v2[1]; v2[1] = v2[2]; v2[2] = w; - w = v3[1]; v3[1] = v3[2]; v3[2] = w; - f = 1; - } - else { - w = v2[0]; v2[0] = v2[2]; v2[2] = w; - w = v3[0]; v3[0] = v3[2]; v3[2] = w; - f = 2; - } - } - if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ) return -1; - k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]); - k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]); - k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]); - k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]); - - a = k1*k1 + k3*k3 + 1; - b = k1*k2 + k3*k4; - c = k2*k2 + k4*k4 - 1; - - d = b*b - a*c; - if( d < 0 ){ - return -1; - } - r3 = (-b + Math.sqrt(d))/a; - p3 = k1*r3 + k2; - q3 = k3*r3 + k4; - r4 = (-b - Math.sqrt(d))/a; - p4 = k1*r4 + k2; - q4 = k3*r4 + k4; - if( f == 1 ) { - w = q3; q3 = r3; r3 = w; - w = q4; q4 = r4; r4 = w; - w = v2[1]; v2[1] = v2[2]; v2[2] = w; - w = v3[1]; v3[1] = v3[2]; v3[2] = w; - f = 0; - } - if( f == 2 ) { - w = p3; p3 = r3; r3 = w; - w = p4; p4 = r4; r4 = w; - w = v2[0]; v2[0] = v2[2]; v2[2] = w; - w = v3[0]; v3[0] = v3[2]; v3[2] = w; - f = 0; - } - - e1 = p1*p3+q1*q3+r1*r3; - if( e1 < 0 ){ - e1 = -e1; - } - e2 = p1*p4+q1*q4+r1*r4; - if( e2 < 0 ){ - e2 = -e2; - } - e3 = p2*p3+q2*q3+r2*r3; - if( e3 < 0 ){ - e3 = -e3; - } - e4 = p2*p4+q2*q4+r2*r4; - if( e4 < 0 ){ - e4 = -e4; - } - if( e1 < e2 ) { - if( e1 < e3 ) { - if( e1 < e4 ) { - rot[0][0] = p1; - rot[0][1] = q1; - rot[0][2] = r1; - rot[1][0] = p3; - rot[1][1] = q3; - rot[1][2] = r3; - } - else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } - else { - if( e3 < e4 ) { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p3; - rot[1][1] = q3; - rot[1][2] = r3; - } - else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } - } - else { - if( e2 < e3 ) { - if( e2 < e4 ) { - rot[0][0] = p1; - rot[0][1] = q1; - rot[0][2] = r1; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } - else { - if( e3 < e4 ) { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p3; - rot[1][1] = q3; - rot[1][2] = r3; - } - else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } - } - return 0; - } - } diff --git a/src/jp/nyatla/nyartoolkit/core/NyARTransRot.java b/src/jp/nyatla/nyartoolkit/core/NyARTransRot.java new file mode 100644 index 0000000..a993a2e --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARTransRot.java @@ -0,0 +1,1198 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +interface NyARTransRot +{ + public double[] getArray(); + /** + * + * @param trans + * @param vertex + * @param pos2d + * [n*2]配列 + * @return + * @throws NyARException + */ + public double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException; + public void initRot(NyARSquare marker_info,int i_direction) throws NyARException; +} + +/** + * NyARTransRot派生クラスで共通に使いそうな関数類をまとめたもの。 + * @author atla + * + */ +abstract class NyARTransRot_OptimizeCommon implements NyARTransRot +{ + protected final int number_of_vertex; + protected final double[] array=new double[9]; + protected final NyARParam cparam; + public final double[] getArray() + { + return this.array; + } + /** + * インスタンスを準備します。 + * @param i_param + * nullを指定した場合、一部の関数が使用不能になります。 + */ + public NyARTransRot_OptimizeCommon(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + number_of_vertex=i_number_of_vertex; + cparam=i_param; + } + + private final double[] wk_check_dir_world=new double[6]; + private final double[] wk_check_dir_camera=new double[4]; + private final NyARMat wk_check_dir_NyARMat=new NyARMat( 3, 3 ); + /** + * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) + * Optimize:STEP[526->468] + * @param dir + * @param st + * @param ed + * @param cpara + * + * @throws NyARException + */ + protected final void check_dir( double dir[], double st[], double ed[],double cpara[]) throws NyARException + { + double h; + int i, j; + + NyARMat mat_a = this.wk_check_dir_NyARMat;//ここ、事前に初期化できそう + double[][] a_array=mat_a.getArray(); + for(j=0;j<3;j++){ + for(i=0;i<3;i++){ + a_array[j][i]=cpara[j*4+i];//m[j*3+i] = cpara[j][i]; + } + + } + // JartkException.trap("未チェックのパス"); + mat_a.matrixSelfInv(); + double[] world=wk_check_dir_world;//[2][3]; + // + //world[0][0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ mat_a->m[2]*10.0; + //world[0][1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ mat_a->m[5]*10.0; + //world[0][2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ mat_a->m[8]*10.0; + //world[1][0] = world[0][0] + dir[0]; + //world[1][1] = world[0][1] + dir[1]; + //world[1][2] = world[0][2] + dir[2]; + world[0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ mat_a->m[2]*10.0; + world[1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ mat_a->m[5]*10.0; + world[2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ mat_a->m[8]*10.0; + world[3] = world[0] + dir[0]; + world[4] = world[1] + dir[1]; + world[5] = world[2] + dir[2]; + // + + double[] camera=wk_check_dir_camera;//[2][2]; + for( i = 0; i < 2; i++ ) { + h = cpara[2*4+0] * world[i*3+0]+ cpara[2*4+1] * world[i*3+1]+ cpara[2*4+2] * world[i*3+2]; + if( h == 0.0 ){ + throw new NyARException(); + } + camera[i*2+0] = (cpara[0*4+0] * world[i*3+0]+ cpara[0*4+1] * world[i*3+1]+ cpara[0*4+2] * world[i*3+2]) / h; + camera[i*2+1] = (cpara[1*4+0] * world[i*3+0]+ cpara[1*4+1] * world[i*3+1]+ cpara[1*4+2] * world[i*3+2]) / h; + } + // + //v[0][0] = ed[0] - st[0]; + //v[0][1] = ed[1] - st[1]; + //v[1][0] = camera[1][0] - camera[0][0]; + //v[1][1] = camera[1][1] - camera[0][1]; + double v=(ed[0]-st[0])*(camera[2]-camera[0])+(ed[1]-st[1])*(camera[3]-camera[1]); + // + if(v<0) {//if( v[0][0]*v[1][0] + v[0][1]*v[1][1] < 0 ) { + dir[0] = -dir[0]; + dir[1] = -dir[1]; + dir[2] = -dir[2]; + } + } + /*int check_rotation( double rot[2][3] )*/ + protected final static void check_rotation( double rot[][] ) throws NyARException + { + double[] v1=new double[3], v2=new double[3], v3=new double[3]; + double ca, cb, k1, k2, k3, k4; + double a, b, c, d; + double p1, q1, r1; + double p2, q2, r2; + double p3, q3, r3; + double p4, q4, r4; + double w; + double e1, e2, e3, e4; + int f; + + v1[0] = rot[0][0]; + v1[1] = rot[0][1]; + v1[2] = rot[0][2]; + v2[0] = rot[1][0]; + v2[1] = rot[1][1]; + v2[2] = rot[1][2]; + v3[0] = v1[1]*v2[2] - v1[2]*v2[1]; + v3[1] = v1[2]*v2[0] - v1[0]*v2[2]; + v3[2] = v1[0]*v2[1] - v1[1]*v2[0]; + w = Math.sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] ); + if( w == 0.0 ){ + throw new NyARException(); + } + v3[0] /= w; + v3[1] /= w; + v3[2] /= w; + + cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; + if( cb < 0 ) cb *= -1.0; + ca = (Math.sqrt(cb+1.0) + Math.sqrt(1.0-cb)) * 0.5; + + if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) { + f = 0; + } + else { + if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) { + w = v1[1]; v1[1] = v1[2]; v1[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 1; + } + else { + w = v1[0]; v1[0] = v1[2]; v1[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 2; + } + } + if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ){ + throw new NyARException(); + } + k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]); + k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]); + k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]); + k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]); + + a = k1*k1 + k3*k3 + 1; + b = k1*k2 + k3*k4; + c = k2*k2 + k4*k4 - 1; + + d = b*b - a*c; + if( d < 0 ){ + throw new NyARException(); + } + r1 = (-b + Math.sqrt(d))/a; + p1 = k1*r1 + k2; + q1 = k3*r1 + k4; + r2 = (-b - Math.sqrt(d))/a; + p2 = k1*r2 + k2; + q2 = k3*r2 + k4; + if( f == 1 ) { + w = q1; q1 = r1; r1 = w; + w = q2; q2 = r2; r2 = w; + w = v1[1]; v1[1] = v1[2]; v1[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 0; + } + if( f == 2 ) { + w = p1; p1 = r1; r1 = w; + w = p2; p2 = r2; r2 = w; + w = v1[0]; v1[0] = v1[2]; v1[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 0; + } + + if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) { + f = 0; + }else { + if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) { + w = v2[1]; v2[1] = v2[2]; v2[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 1; + } + else { + w = v2[0]; v2[0] = v2[2]; v2[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 2; + } + } + if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ){ + throw new NyARException(); + } + k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]); + k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]); + k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]); + k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]); + + a = k1*k1 + k3*k3 + 1; + b = k1*k2 + k3*k4; + c = k2*k2 + k4*k4 - 1; + + d = b*b - a*c; + if( d < 0 ){ + throw new NyARException(); + } + r3 = (-b + Math.sqrt(d))/a; + p3 = k1*r3 + k2; + q3 = k3*r3 + k4; + r4 = (-b - Math.sqrt(d))/a; + p4 = k1*r4 + k2; + q4 = k3*r4 + k4; + if( f == 1 ) { + w = q3; q3 = r3; r3 = w; + w = q4; q4 = r4; r4 = w; + w = v2[1]; v2[1] = v2[2]; v2[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 0; + } + if( f == 2 ) { + w = p3; p3 = r3; r3 = w; + w = p4; p4 = r4; r4 = w; + w = v2[0]; v2[0] = v2[2]; v2[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 0; + } + + e1 = p1*p3+q1*q3+r1*r3; + if( e1 < 0 ){ + e1 = -e1; + } + e2 = p1*p4+q1*q4+r1*r4; + if( e2 < 0 ){ + e2 = -e2; + } + e3 = p2*p3+q2*q3+r2*r3; + if( e3 < 0 ){ + e3 = -e3; + } + e4 = p2*p4+q2*q4+r2*r4; + if( e4 < 0 ){ + e4 = -e4; + } + if( e1 < e2 ) { + if( e1 < e3 ) { + if( e1 < e4 ) { + rot[0][0] = p1; + rot[0][1] = q1; + rot[0][2] = r1; + rot[1][0] = p3; + rot[1][1] = q3; + rot[1][2] = r3; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + else { + if( e3 < e4 ) { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p3; + rot[1][1] = q3; + rot[1][2] = r3; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + } + else { + if( e2 < e3 ) { + if( e2 < e4 ) { + rot[0][0] = p1; + rot[0][1] = q1; + rot[0][2] = r1; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + else { + if( e3 < e4 ) { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p3; + rot[1][1] = q3; + rot[1][2] = r3; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + } + } + /** + * パラメタa,b,cからrotを計算してインスタンスに保存する。 + * rotを1次元配列に変更 + * Optimize:2008.04.20:STEP[253→186] + * @param a + * @param b + * @param c + * @param o_rot + */ + protected final static void arGetRot( double a, double b, double c,double[] o_rot) + { + double sina, sinb, sinc; + double cosa, cosb, cosc; + + sina = Math.sin(a); + cosa = Math.cos(a); + sinb = Math.sin(b); + cosb = Math.cos(b); + sinc = Math.sin(c); + cosc = Math.cos(c); + //Optimize + double CACA,SASA,SACA,SASB,CASB; + CACA=cosa*cosa; + SASA=sina*sina; + SACA=sina*cosa; + SASB=sina*sinb; + CASB=cosa*sinb; + + o_rot[0] = CACA*cosb*cosc+SASA*cosc+SACA*cosb*sinc-SACA*sinc; + o_rot[1] = -CACA*cosb*sinc-SASA*sinc+SACA*cosb*cosc-SACA*cosc; + o_rot[2] = CASB; + o_rot[3] = SACA*cosb*cosc-SACA*cosc+SASA*cosb*sinc+CACA*sinc; + o_rot[4] = -SACA*cosb*sinc+SACA*sinc+SASA*cosb*cosc+CACA*cosc; + o_rot[5] = SASB; + o_rot[6] = -CASB*cosc-SASB*sinc; + o_rot[7] = CASB*sinc-SASB*cosc; + o_rot[8] = cosb; + } + /** + * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) + * Optimize:2008.04.20:STEP[481→433] + * @param rot + * 2次元配列を1次元化してあります。 + * @param o_abc + * @return + */ + protected final int arGetAngle(double[] o_abc) + { + double a, b, c; + double sina, cosa, sinb, cosb, sinc, cosc; + double[] rot=array; + if( rot[8] > 1.0 ) {//if( rot[2][2] > 1.0 ) { + rot[8] = 1.0;//rot[2][2] = 1.0; + }else if( rot[8] < -1.0 ) {//}else if( rot[2][2] < -1.0 ) { + rot[8] = -1.0;//rot[2][2] = -1.0; + } + cosb = rot[8];//cosb = rot[2][2]; + b = Math.acos( cosb ); + sinb = Math.sin( b ); + if( b >= 0.000001 || b <= -0.000001) { + cosa = rot[2] / sinb;//cosa = rot[0][2] / sinb; + sina = rot[5] / sinb;//sina = rot[1][2] / sinb; + if( cosa > 1.0 ) { + /* printf("cos(alph) = %f\n", cosa); */ + cosa = 1.0; + sina = 0.0; + } + if( cosa < -1.0 ) { + /* printf("cos(alph) = %f\n", cosa); */ + cosa = -1.0; + sina = 0.0; + } + if( sina > 1.0 ) { + /* printf("sin(alph) = %f\n", sina); */ + sina = 1.0; + cosa = 0.0; + } + if( sina < -1.0 ) { + /* printf("sin(alph) = %f\n", sina); */ + sina = -1.0; + cosa = 0.0; + } + a = Math.acos( cosa ); + if( sina < 0 ){ + a = -a; + } + // + //sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]); + //cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]); + sinc = (rot[7]*rot[2]-rot[6]*rot[5])/ (rot[2]*rot[2]+rot[5]*rot[5]); + cosc = -(rot[2]*rot[6]+rot[5]*rot[7])/ (rot[2]*rot[2]+rot[5]*rot[5]); + // + + if( cosc > 1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = 1.0; + sinc = 0.0; + } + if( cosc < -1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = -1.0; + sinc = 0.0; + } + if( sinc > 1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = 1.0; + cosc = 0.0; + } + if( sinc < -1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = -1.0; + cosc = 0.0; + } + c = Math.acos( cosc ); + if( sinc < 0 ){ + c = -c; + } + }else { + a = b = 0.0; + cosa = cosb = 1.0; + sina = sinb = 0.0; + cosc = rot[0];//cosc = rot[0][0]; + sinc = rot[1];//sinc = rot[1][0]; + if( cosc > 1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = 1.0; + sinc = 0.0; + } + if( cosc < -1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = -1.0; + sinc = 0.0; + } + if( sinc > 1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = 1.0; + cosc = 0.0; + } + if( sinc < -1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = -1.0; + cosc = 0.0; + } + c = Math.acos( cosc ); + if( sinc < 0 ){ + c = -c; + } + } + o_abc[0]=a;//wa.value=a;//*wa = a; + o_abc[1]=b;//wb.value=b;//*wb = b; + o_abc[2]=c;//wc.value=c;//*wc = c; + return 0; + } +} + +/** + * NyARModifyMatrixの最適化バージョン1 + * 配列の１次元化、計算ステップの圧縮等の最適化をしてみた。 + * + */ +class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon +{ + public NyARTransRot_O1(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + super(i_param,i_number_of_vertex); + } + /** + * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] ) + * Optimize:2008.04.20:STEP[716→698] + * @param marker_info + * @param i_direction + * @param i_param + * @throws NyARException + */ + public final void initRot(NyARSquare marker_info,int i_direction) throws NyARException + { + double cpara[]= cparam.get34Array(); + double[][] wdir=new double[3][3]; + double w, w1, w2, w3; + int dir; + int j; + + dir = i_direction; + + for( j = 0; j < 2; j++ ) { + w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1]; + w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2]; + w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0]; + + wdir[j][0] = w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+ w2*cpara[1*4+1]- w3*cpara[0*4+1]; + wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+ w3*cpara[0*4+0]; + wdir[j][2] = w1*cpara[0*4+0]*cpara[1*4+1]; + w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] ); + wdir[j][0] /= w; + wdir[j][1] /= w; + wdir[j][2] /= w; + } + + //以下3ケースは、計算エラーのときは例外が発生する。 + check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara); + + check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara); + + check_rotation(wdir); + + + wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1]; + wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2]; + wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0]; + w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] ); + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + /* + if( wdir[2][2] < 0 ) { + wdir[2][0] /= -w; + wdir[2][1] /= -w; + wdir[2][2] /= -w; + } + else { + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + } + */ + // + //rot[0][0] = wdir[0][0]; + //rot[1][0] = wdir[0][1]; + //rot[2][0] = wdir[0][2]; + //rot[0][1] = wdir[1][0]; + //rot[1][1] = wdir[1][1]; + //rot[2][1] = wdir[1][2]; + //rot[0][2] = wdir[2][0]; + //rot[1][2] = wdir[2][1]; + //rot[2][2] = wdir[2][2]; + double[] rot=this.array; + rot[0] = wdir[0][0]; + rot[3] = wdir[0][1]; + rot[6] = wdir[0][2]; + rot[1] = wdir[1][0]; + rot[4] = wdir[1][1]; + rot[7] = wdir[1][2]; + rot[2] = wdir[2][0]; + rot[5] = wdir[2][1]; + rot[8] = wdir[2][2]; + // + } + private final double[] wk_arModifyMatrix_combo=new double[12];//[3][4]; + private final double[] wk_arModifyMatrix_abc=new double[3]; + private final double[] wk_arModifyMatrix_rot=new double[9]; + /** + * Optimize:2008.04.20:STEP[456→-] + * @param rot + * [3x3]配列 + * @param trans + * @param vertex + * @param pos2d + * @param num + * @return + */ + public final double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException + { + int num=this.number_of_vertex; + double factor; + double a1, b1, c1; + double a2, b2, c2; + double ma = 0.0, mb = 0.0, mc = 0.0; + double hx, hy, h, x, y; + double err, minerr=0; + int t1, t2, t3; + int s1 = 0, s2 = 0, s3 = 0; + int i, j; + double[] combo=this.wk_arModifyMatrix_combo;//arGetNewMatrixで初期化されるので初期化不要//new double[3][4]; + double[] abc=wk_arModifyMatrix_abc; + double[] rot=wk_arModifyMatrix_rot; + + arGetAngle(abc);//arGetAngle( rot, &a, &b, &c ); + a2 = abc[0]; + b2 = abc[1]; + c2 = abc[2]; + factor = 10.0*Math.PI/180.0; + for( j = 0; j < 10; j++ ) { + minerr = 1000000000.0; + for(t1=-1;t1<=1;t1++) { + for(t2=-1;t2<=1;t2++) { + for(t3=-1;t3<=1;t3++) { + a1 = a2 + factor*t1; + b1 = b2 + factor*t2; + c1 = c2 + factor*t3; + arGetRot( a1, b1, c1,rot); + arGetNewMatrix(rot,trans, null, combo ); + err = 0.0; + for( i = 0; i < num; i++ ) { + hx = combo[0] * vertex[i*3+0]+ combo[1] * vertex[i*3+1]+ combo[2] * vertex[i*3+2]+ combo[3]; + hy = combo[4] * vertex[i*3+0]+ combo[5] * vertex[i*3+1]+ combo[6] * vertex[i*3+2]+ combo[7]; + h = combo[8] * vertex[i*3+0]+ combo[9] * vertex[i*3+1]+ combo[10] * vertex[i*3+2]+ combo[11]; + x = hx / h; + y = hy / h; + err += (pos2d[i*2+0] - x) * (pos2d[i*2+0] - x)+ (pos2d[i*2+1] - y) * (pos2d[i*2+1] - y); + } + if( err < minerr ) { + minerr = err; + ma = a1; + mb = b1; + mc = c1; + s1 = t1; + s2 = t2; + s3 = t3; + } + } + } + } + if( s1 == 0 && s2 == 0 && s3 == 0 ){ + factor *= 0.5; + } + a2 = ma; + b2 = mb; + c2 = mc; + } + arGetRot(ma, mb, mc,this.array); + /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ + return minerr/num; + } + private final double[] wk_cpara2_arGetNewMatrix=new double[12];//[3][4]; + /** + * Optimize:2008.04.20:STEP[569->432] + * @param i_rot + * [9] + * @param trans + * @param trans2 + * @param ret + * double[3x4]配列 + * @return + */ + private final int arGetNewMatrix(double[] i_rot,double trans[], double trans2[][], double ret[]) throws NyARException + { + final double cpara[]=cparam.get34Array(); + final double[] cpara2; //この関数で初期化される。 + int j,j_idx; +// double[] cpara_pt; + //cparaの2次元配列→1次元に変換して計算 + if( trans2 != null ) { + cpara2=wk_cpara2_arGetNewMatrix; //この関数で初期化される。 + + for( j = 0; j < 3; j++ ) { +// Optimize(使わないから最適化してない) + NyARException.trap("未チェックのパス"); + cpara2[j*4+0] = cpara[j*4+0] * trans2[0][0]+ cpara[j*4+1] * trans2[1][0]+ cpara[j*4+2] * trans2[2][0]; + cpara2[j*4+1] = cpara[j*4+0] * trans2[0][1]+ cpara[j*4+1] * trans2[1][1]+ cpara[j*4+2] * trans2[2][1]; + cpara2[j*4+2] = cpara[j*4+0] * trans2[0][2]+ cpara[j*4+1] * trans2[1][2]+ cpara[j*4+2] * trans2[2][2]; + cpara2[j*4+3] = cpara[j*4+0] * trans2[0][3]+ cpara[j*4+1] * trans2[1][3]+ cpara[j*4+2] * trans2[2][3]; + } + }else{ + cpara2=cpara;//cparaの値をそのまま使う + } + for( j = 0; j < 3; j++ ) { + //cpara2_pt=cpara2[j]; + j_idx=j*4; + // + //ret[j][0] = cpara2_pt[0] * rot[0][0]+ cpara2_pt[1] * rot[1][0]+ cpara2_pt[2] * rot[2][0]; + //ret[j][1] = cpara2_pt[0] * rot[0][1]+ cpara2_pt[1] * rot[1][1]+ cpara2_pt[2] * rot[2][1]; + //ret[j][2] = cpara2_pt[0] * rot[0][2]+ cpara2_pt[1] * rot[1][2]+ cpara2_pt[2] * rot[2][2]; + //ret[j][3] = cpara2_pt[0] * trans[0]+ cpara2_pt[1] * trans[1]+ cpara2_pt[2] * trans[2]+ cpara2_pt[3]; + ret[j_idx+0] = cpara2[j_idx+0] * i_rot[0]+ cpara2[j_idx+1] * i_rot[3]+ cpara2[j_idx+2] * i_rot[6]; + ret[j_idx+1] = cpara2[j_idx+0] * i_rot[1]+ cpara2[j_idx+1] * i_rot[4]+ cpara2[j_idx+2] * i_rot[7]; + ret[j_idx+2] = cpara2[j_idx+0] * i_rot[2]+ cpara2[j_idx+1] * i_rot[5]+ cpara2[j_idx+2] * i_rot[8]; + ret[j_idx+3] = cpara2[j_idx+0] * trans[0]+ cpara2[j_idx+1] * trans[1]+ cpara2[j_idx+2] * trans[2]+ cpara2[j_idx+3]; + // + } + return(0); + } +} + +/** + * NyARModifyMatrixの最適化バージョン2 + * 計算手順の変更、構造変更など含む最適化をしたもの + * + */ +class NyARTransRot_O2 extends NyARTransRot_OptimizeCommon +{ + public NyARTransRot_O2(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + super(i_param,i_number_of_vertex); + } + + //private double CACA,SASA,SACA,CA,SA; + private double CACA,SASA,SACA,CA,SA; + final public void arGetRotA( double a) + { + double sina,cosa; + sina = Math.sin(a); + cosa = Math.cos(a); + //Optimize + CACA=cosa*cosa; + SASA=sina*sina; + SACA=sina*cosa; + CA=cosa; + SA=sina; + } + private double CACACB,SACACB,SASACB,CASB,SASB; + final public void arGetRotB(double b,double[] o_rot) + { + double sinb,cosb; + sinb = Math.sin(b); + cosb = Math.cos(b); + CACACB=CACA*cosb; + SACACB=SACA*cosb; + SASACB=SASA*cosb; + CASB=CA*sinb; + SASB=SA*sinb; + o_rot[2] = CASB; + o_rot[5] = SASB; + o_rot[8] = cosb; + } + /** + * 分割arGetRot + * @param c + */ + public final void arGetRotC(double c,double[] o_rot) + { + double sinc,cosc; + sinc = Math.sin(c); + cosc = Math.cos(c); + double SACASC,SACACBSC,SACACBCC,SACACC; + SACASC=SACA*sinc; + SACACC=SACA*cosc; + SACACBSC=SACACB*sinc; + SACACBCC=SACACB*cosc; + o_rot[0] = CACACB*cosc+SASA*cosc+SACACBSC-SACASC; + o_rot[1] = -CACACB*sinc-SASA*sinc+SACACBCC-SACACC; + o_rot[3] = SACACBCC-SACACC+SASACB*sinc+CACA*sinc; + o_rot[4] = -SACACBSC+SACASC+SASACB*cosc+CACA*cosc; + o_rot[6] = -CASB*cosc-SASB*sinc; + o_rot[7] = CASB*sinc-SASB*cosc; + } + private final double[][] wk_initRot_wdir=new double[3][3]; + /** + * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] ) + * Optimize:2008.04.20:STEP[716→698] + * @param marker_info + * @param i_direction + * @param i_param + * @throws NyARException + */ + public void initRot(NyARSquare marker_info,int i_direction) throws NyARException + { + double cpara[]= cparam.get34Array(); + double[][] wdir=wk_initRot_wdir;//この関数で初期化される + double w, w1, w2, w3; + int dir; + int j; + + dir = i_direction; + + for( j = 0; j < 2; j++ ) { + w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1]; + w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2]; + w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0]; + + wdir[j][0] = w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+ w2*cpara[1*4+1]- w3*cpara[0*4+1]; + wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+ w3*cpara[0*4+0]; + wdir[j][2] = w1*cpara[0*4+0]*cpara[1*4+1]; + w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] ); + wdir[j][0] /= w; + wdir[j][1] /= w; + wdir[j][2] /= w; + } + + //以下3ケースは、計算エラーのときは例外が発生する。 + check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara); + + check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara); + + check_rotation(wdir); + + + wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1]; + wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2]; + wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0]; + w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] ); + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + // + //rot[0][0] = wdir[0][0]; + //rot[1][0] = wdir[0][1]; + //rot[2][0] = wdir[0][2]; + //rot[0][1] = wdir[1][0]; + //rot[1][1] = wdir[1][1]; + //rot[2][1] = wdir[1][2]; + //rot[0][2] = wdir[2][0]; + //rot[1][2] = wdir[2][1]; + //rot[2][2] = wdir[2][2]; + double[] rot=this.array; + rot[0] = wdir[0][0]; + rot[3] = wdir[0][1]; + rot[6] = wdir[0][2]; + rot[1] = wdir[1][0]; + rot[4] = wdir[1][1]; + rot[7] = wdir[1][2]; + rot[2] = wdir[2][0]; + rot[5] = wdir[2][1]; + rot[8] = wdir[2][2]; + // + } + private final double[] wk_arModifyMatrix_combo=new double[12];//[3][4]; + private final double[] wk_arModifyMatrix_abc=new double[3]; + private final double[] wk_arModifyMatrix_rot=new double[9]; + /** + * arGetRot計算を階層化したModifyMatrix + * @param nyrot + * @param trans + * @param vertex + * @param pos2d + * @param num + * @return + * @throws NyARException + */ + public double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException + { + int num=this.number_of_vertex; + double factor; + double a1, b1, c1; + double a2, b2, c2; + double ma = 0.0, mb = 0.0, mc = 0.0; + double hx, hy, h, x, y; + double err, minerr=0; + int t1, t2, t3; + int s1 = 0, s2 = 0, s3 = 0; + int i, j; + final double[] combo=this.wk_arModifyMatrix_combo;//arGetNewMatrixで初期化されるので初期化不要//new double[3][4]; + final double[] abc=wk_arModifyMatrix_abc; + double[] rot=wk_arModifyMatrix_rot; + + arGetAngle(abc);//arGetAngle( rot, &a, &b, &c ); + a2 = abc[0]; + b2 = abc[1]; + c2 = abc[2]; + factor = 10.0*Math.PI/180.0; + + nyatla_arGetNewMatrix_row3(trans,combo);//comboの3行目を先に計算 + for( j = 0; j < 10; j++ ) { + minerr = 1000000000.0; + for(t1=-1;t1<=1;t1++) { + a1 = a2 + factor*t1; + arGetRotA(a1); + for(t2=-1;t2<=1;t2++) { + b1 = b2 + factor*t2; + arGetRotB(b1,rot); + for(t3=-1;t3<=1;t3++) { + c1 = c2 + factor*t3; + arGetRotC(c1,rot); + //comboの0-2行目を計算 + nyatla_arGetNewMatrix_row012(rot,trans,combo);//第二パラメタは常にnull//arGetNewMatrix(trans, null, combo ); + err = 0.0; + for( i = 0; i < num; i++ ) { + hx = combo[0] * vertex[i*3+0]+ combo[1] * vertex[i*3+1]+ combo[2] * vertex[i*3+2]+ combo[3]; + hy = combo[4] * vertex[i*3+0]+ combo[5] * vertex[i*3+1]+ combo[6] * vertex[i*3+2]+ combo[7]; + h = combo[8] * vertex[i*3+0]+ combo[9] * vertex[i*3+1]+ combo[10] * vertex[i*3+2]+ combo[11]; + x = hx / h; + y = hy / h; + err += (pos2d[i*2+0] - x) * (pos2d[i*2+0] - x)+ (pos2d[i*2+1] - y) * (pos2d[i*2+1] - y); + } + if( err < minerr ) { + minerr = err; + ma = a1; + mb = b1; + mc = c1; + s1 = t1; + s2 = t2; + s3 = t3; + } + } + } + } + if( s1 == 0 && s2 == 0 && s3 == 0 ){ + factor *= 0.5; + } + a2 = ma; + b2 = mb; + c2 = mc; + } + arGetRot(ma,mb,mc,this.array); + /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ + return minerr/num; + } + /** + * arGetNewMatrixの0-2行目を初期化する関数 + * Optimize:2008.04.20:STEP[569->144] + * @param i_rot + * @param trans + * @param trans2 + * @param ret + * double[3x4]配列 + * @return + */ + private final void nyatla_arGetNewMatrix_row012(double i_rot[],double trans[],double ret[]) throws NyARException + { + int j; + double c0,c1,c2; + final double cpara2[]=cparam.get34Array(); + for( j = 0; j < 3; j++ ) { + //cpara2_pt=cpara2[j]; + c0=cpara2[j*4+0]; + c1=cpara2[j*4+1]; + c2=cpara2[j*4+2]; + ret[j*4+0] = c0 * i_rot[0]+ c1 * i_rot[3]+ c2 * i_rot[6]; + ret[j*4+1] = c0 * i_rot[1]+ c1 * i_rot[4]+ c2 * i_rot[7]; + ret[j*4+2] = c0 * i_rot[2]+ c1 * i_rot[5]+ c2 * i_rot[8]; + // + } + return; + } + /** + * arGetNewMatrixの3行目を初期化する関数 + * @param trans + * @param ret + * @throws NyARException + */ + private final void nyatla_arGetNewMatrix_row3(double trans[],double ret[]) throws NyARException + { + final double cpara2[]=cparam.get34Array(); + int j,j_idx; + for( j = 0; j < 3; j++ ) { + j_idx=j*4; + ret[j_idx+3] = cpara2[j_idx+0] * trans[0]+ cpara2[j_idx+1] * trans[1]+ cpara2[j_idx+2] * trans[2]+ cpara2[j_idx+3]; + } + return; + } +} + + +/** + * NyARModifyMatrixの最適化バージョン3 + * 計算速度のみを追求する + * + */ +class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon +{ + public NyARTransRot_O3(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + super(i_param,i_number_of_vertex); + if(i_number_of_vertex!=4){ + //4以外の頂点数は処理しない + throw new NyARException(); + } + } + + //private double CACA,SASA,SACA,CA,SA; + private final double[][] wk_initRot_wdir=new double[3][3]; + /** + * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] ) + * Optimize:2008.04.20:STEP[716→698] + * @param marker_info + * @param i_direction + * @param i_param + * @throws NyARException + */ + public void initRot(NyARSquare marker_info,int i_direction) throws NyARException + { + double cpara[]= cparam.get34Array(); + double[][] wdir=wk_initRot_wdir;//この関数で初期化される + double w, w1, w2, w3; + int dir; + int j; + + dir = i_direction; + + for( j = 0; j < 2; j++ ) { + w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1]; + w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2]; + w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0]; + + wdir[j][0] = w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+ w2*cpara[1*4+1]- w3*cpara[0*4+1]; + wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+ w3*cpara[0*4+0]; + wdir[j][2] = w1*cpara[0*4+0]*cpara[1*4+1]; + w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] ); + wdir[j][0] /= w; + wdir[j][1] /= w; + wdir[j][2] /= w; + } + + //以下3ケースは、計算エラーのときは例外が発生する。 + check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara); + + check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara); + + check_rotation(wdir); + + + wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1]; + wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2]; + wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0]; + w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] ); + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + double[] rot=this.array; + rot[0] = wdir[0][0]; + rot[3] = wdir[0][1]; + rot[6] = wdir[0][2]; + rot[1] = wdir[1][0]; + rot[4] = wdir[1][1]; + rot[7] = wdir[1][2]; + rot[2] = wdir[2][0]; + rot[5] = wdir[2][1]; + rot[8] = wdir[2][2]; + // + } + private final double[] wk_arModifyMatrix_abc=new double[3]; + /** + * arGetRot計算を階層化したModifyMatrix + * 896 + * @param nyrot + * @param trans + * @param vertex + * @param pos2d + * @param num + * @return + * @throws NyARException + */ + public double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException + { + double CACA,SASA,SACA,CA,SA; + double CACACB,SACACB,SASACB,CASB,SASB; + double SACASC,SACACBSC,SACACBCC,SACACC; + double factor; + double a1, b1, c1; + double a2, b2, c2; + double ma = 0.0, mb = 0.0, mc = 0.0; + double h, x, y; + double err, minerr=0; + int t1, t2, t3; + int s1 = 0, s2 = 0, s3 = 0; + int i; + final double[] abc=wk_arModifyMatrix_abc; + + arGetAngle(abc);//arGetAngle( rot, &a, &b, &c ); + a2 = abc[0]; + b2 = abc[1]; + c2 = abc[2]; + factor = 10.0*Math.PI/180.0; + double rot0,rot1,rot3,rot4,rot6,rot7; + double combo00,combo01,combo02,combo03,combo10,combo11,combo12,combo13,combo20,combo21,combo22,combo23; + double combo02_2,combo02_5,combo02_8,combo02_11; + double combo22_2,combo22_5,combo22_8,combo22_11; + double combo12_2,combo12_5,combo12_8,combo12_11; + final double cpara[]=cparam.get34Array(); + combo03 = cpara[0] * trans[0]+ cpara[1] * trans[1]+ cpara[2] * trans[2]+ cpara[3]; + combo13 = cpara[4] * trans[0]+ cpara[5] * trans[1]+ cpara[6] * trans[2]+ cpara[7]; + combo23 = cpara[8] * trans[0]+ cpara[9] * trans[1]+ cpara[10] * trans[2]+ cpara[11]; + double wsin,wcos; + //comboの3行目を先に計算 + for( i = 0; i < 10; i++ ) { + minerr = 1000000000.0; + for(t1=-1;t1<=1;t1++) { + a1 = a2 + factor*t1; + wsin = Math.sin(a1); + wcos = Math.cos(a1); + //Optimize + CACA=wcos*wcos; + SASA=wsin*wsin; + SACA=wsin*wcos; + CA=wcos; + SA=wsin; + for(t2=-1;t2<=1;t2++) { + b1 = b2 + factor*t2; + wsin = Math.sin(b1); + wcos = Math.cos(b1); + CACACB=CACA*wcos; + SACACB=SACA*wcos; + SASACB=SASA*wcos; + CASB=CA*wsin; + SASB=SA*wsin; + //comboの計算1 + combo02 = cpara[0] * CASB+ cpara[1] * SASB+ cpara[2] * wcos; + combo12 = cpara[4] * CASB+ cpara[5] * SASB+ cpara[6] * wcos; + combo22 = cpara[8] * CASB+ cpara[9] * SASB+ cpara[10] * wcos; + combo02_2 =combo02 * vertex[2]+combo03; + combo02_5 =combo02 * vertex[5]+combo03; + combo02_8 =combo02 * vertex[8]+combo03; + combo02_11=combo02 * vertex[11]+combo03; + combo12_2 =combo12 * vertex[2]+ combo13; + combo12_5 =combo12 * vertex[5]+ combo13; + combo12_8 =combo12 * vertex[8]+ combo13; + combo12_11=combo12 * vertex[11]+ combo13; + combo22_2 =combo22 * vertex[2]+ combo23; + combo22_5 =combo22 * vertex[5]+ combo23; + combo22_8 =combo22 * vertex[8]+ combo23; + combo22_11=combo22 * vertex[11]+ combo23; + + for(t3=-1;t3<=1;t3++) { + c1 = c2 + factor*t3; + wsin = Math.sin(c1); + wcos = Math.cos(c1); + SACASC=SACA*wsin; + SACACC=SACA*wcos; + SACACBSC=SACACB*wsin; + SACACBCC=SACACB*wcos; + + rot0 = CACACB*wcos+SASA*wcos+SACACBSC-SACASC; + rot3 = SACACBCC-SACACC+SASACB*wsin+CACA*wsin; + rot6 = -CASB*wcos-SASB*wsin; + combo00 = cpara[0] * rot0+ cpara[1] * rot3+ cpara[2] * rot6; + combo10 = cpara[4] * rot0+ cpara[5] * rot3+ cpara[6] * rot6; + combo20 = cpara[8] * rot0+ cpara[9] * rot3+ cpara[10] * rot6; + + rot1 = -CACACB*wsin-SASA*wsin+SACACBCC-SACACC; + rot4 = -SACACBSC+SACASC+SASACB*wcos+CACA*wcos; + rot7 = CASB*wsin-SASB*wcos; + combo01 = cpara[0] * rot1+ cpara[1] * rot4+ cpara[2] * rot7; + combo11 = cpara[4] * rot1+ cpara[5] * rot4+ cpara[6] * rot7; + combo21 = cpara[8] * rot1+ cpara[9] * rot4+ cpara[10] * rot7; + // + err = 0.0; + h = combo20 * vertex[0]+ combo21 * vertex[1]+ combo22_2; + x = pos2d[0] - (combo00 * vertex[0]+ combo01 * vertex[1]+ combo02_2) / h; + y = pos2d[1] - (combo10 * vertex[0]+ combo11 * vertex[1]+ combo12_2) / h; + err += x*x+y*y; + h = combo20 * vertex[3]+ combo21 * vertex[4]+ combo22_5; + x = pos2d[2] - (combo00 * vertex[3]+ combo01 * vertex[4]+ combo02_5) / h; + y = pos2d[3] - (combo10 * vertex[3]+ combo11 * vertex[4]+ combo12_5) / h; + err += x*x+y*y; + h = combo20 * vertex[6]+ combo21 * vertex[7]+ combo22_8; + x = pos2d[4] - (combo00 * vertex[6]+ combo01 * vertex[7]+ combo02_8) / h; + y = pos2d[5] - (combo10 * vertex[6]+ combo11 * vertex[7]+ combo12_8) / h; + err += x*x+y*y; + h = combo20 * vertex[9]+ combo21 * vertex[10]+ combo22_11; + x = pos2d[6] - (combo00 * vertex[9]+ combo01 * vertex[10]+ combo02_11) / h; + y = pos2d[7] - (combo10 * vertex[9]+ combo11 * vertex[10]+ combo12_11) / h; + err += x*x+y*y; + if( err < minerr ) { + minerr = err; + ma = a1; + mb = b1; + mc = c1; + s1 = t1; + s2 = t2; + s3 = t3; + } + } + } + } + if( s1 == 0 && s2 == 0 && s3 == 0 ){ + factor *= 0.5; + } + a2 = ma; + b2 = mb; + c2 = mc; + } + arGetRot(ma,mb,mc,this.array); + /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ + return minerr/4; + } +} + diff --git a/src/jp/nyatla/nyartoolkit/core/NyARVec.java b/src/jp/nyatla/nyartoolkit/core/NyARVec.java index 53a22ff..66b81f2 100644 --- a/src/jp/nyatla/nyartoolkit/core/NyARVec.java +++ b/src/jp/nyatla/nyartoolkit/core/NyARVec.java @@ -38,17 +38,28 @@ import jp.nyatla.nyartoolkit.NyARException; public class NyARVec { private int clm; - private NyARVec(double[] i_array) - { - v=i_array; - clm=v.length; - } public NyARVec(int i_clm) { v=new double[i_clm]; clm=i_clm; } private double[] v; + /** + * i_clmサイズの列を格納できるように列サイズを変更します。 + * 実行後、列の各値は不定になります。 + * @param i_clm + */ + public void realloc(int i_clm) + { + if(i_clm<=this.v.length) + { + //十分な配列があれば何もしない。 + }else{ + //不十分なら取り直す。 + v=new double[i_clm]; + } + this.clm=i_clm; + } public int getClm() { return clm; @@ -57,13 +68,9 @@ public class NyARVec { return v; } - public static NyARVec wrap(double[] i_array) - { - return new NyARVec(i_array); - } /** * arVecDispの代替品 - * @param v + * @param value * @return */ public int arVecDisp() throws NyARException @@ -87,19 +94,19 @@ public class NyARVec * @return * @throws NyARException */ - private static double vecInnerproduct(NyARVec x,NyARVec y,int i_start) throws NyARException + public double vecInnerproduct(NyARVec y,int i_start) throws NyARException { NyARException.trap("この関数は動作確認できていません。"); double result = 0.0; // double[] x_array=x.v;.getArray(); // double[] y_array=y.getArray(); - if(x.clm!= y.clm){ + if(this.clm!= y.clm){ throw new NyARException();//exit(); } - for(int i = i_start; i < x.clm; i++ ) { + for(int i = i_start; i < this.clm; i++ ) { NyARException.trap("未チェックのパス"); - result += x.v[i] * y.v[i];//result += x->v[i] * y->v[i]; + result += this.v[i] * y.v[i];//result += x->v[i] * y->v[i]; } return result; } @@ -111,133 +118,143 @@ public class NyARVec * @return * @throws NyARException */ - private static double vecHousehold(NyARVec x,int i_start) throws NyARException + public double vecHousehold(int i_start) throws NyARException { NyARException.trap("この関数は動作確認できていません。"); double s, t; - s = Math.sqrt(vecInnerproduct(x,x,i_start)); + s = Math.sqrt(this.vecInnerproduct(this,i_start)); // double[] x_array=x.getArray(); if( s != 0.0 ){ NyARException.trap("未チェックのパス"); - if(x.v[i_start]< 0){ + if(this.v[i_start]< 0){ s = -s; } NyARException.trap("未チェックのパス");{ - x.v[i_start]+=s;//x->v[0] += s; - t = 1 / Math.sqrt(x.v[i_start]* s);//t = 1 / sqrt(x->v[0] * s); + this.v[i_start]+=s;//x->v[0] += s; + t = 1 / Math.sqrt(this.v[i_start]* s);//t = 1 / sqrt(x->v[0] * s); } - for(int i = i_start; i < x.clm; i++){ + for(int i = i_start; i < this.clm; i++){ NyARException.trap("未チェックのパス"); - x.v[i]*=t;//x->v[i] *= t; + this.v[i]*=t;//x->v[i] *= t; } } return -s; } +// /** +// * arVecTridiagonalize関数の代替品 +// * a,d,e間で演算をしてる。何をどうしているかはさっぱりさっぱり +// * @param a +// * @param d +// * @param e +// * @param i_e_start +// * 演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる) +// * @return +// * @throws NyARException +// */ +// public static void vecTridiagonalize(NyARMat a, NyARVec d, NyARVec e,int i_e_start) throws NyARException +// { +// NyARVec vec,vec2; +// double[][] a_array=a.getArray(); +// double s, t, p, q; +// int dim; +// +// if(a.getClm()!=a.getRow()){ +// throw new NyARException(); +// } +// if(a.getClm() != d.clm){ +// throw new NyARException(); +// } +// if(a.getClm() != e.clm){ +// throw new NyARException(); +// } +// dim = a.getClm(); +// +// for(int k = 0; k < dim-2; k++ ){ +// vec=a.getRowVec(k); +//// double[] vec_array=vec.getArray(); +// NyARException.trap("未チェックパス"); +// d.v[k]=vec.v[k];//d.set(k,v.get(k)); //d->v[k] = v[k]; +// +// //wv1.clm = dim-k-1; +// //wv1.v = &(v[k+1]); +// NyARException.trap("未チェックパス"); +// e.v[k+i_e_start]=vec.vecHousehold(k+1);//e->v[k] = arVecHousehold(&wv1); +// if(e.v[k+i_e_start]== 0.0 ){ +// continue; +// } +// +// for(int i = k+1; i < dim; i++ ){ +// s = 0.0; +// for(int j = k+1; j < i; j++ ) { +// NyARException.trap("未チェックのパス"); +// s += a_array[j][i] * vec.v[j];//s += a.get(j*dim+i) * v.get(j);//s += a->m[j*dim+i] * v[j]; +// } +// for(int j = i; j < dim; j++ ) { +// NyARException.trap("未チェックのパス"); +// s += a_array[i][j] * vec.v[j];//s += a.get(i*dim+j) * v.get(j);//s += a->m[i*dim+j] * v[j]; +// } +// NyARException.trap("未チェックのパス"); +// d.v[i]=s;//d->v[i] = s; +// } +// +// +// //wv1.clm = wv2.clm = dim-k-1; +// //wv1.v = &(v[k+1]); +// //wv2.v = &(d->v[k+1]); +// vec=a.getRowVec(k); +//// vec_array=vec.getArray(); +// NyARException.trap("未チェックパス"); +// t = vec.vecInnerproduct(d,k+1)/ 2; +// for(int i = dim-1; i > k; i-- ) { +// NyARException.trap("未チェックパス"); +// p = vec.v[i];//p = v.get(i);//p = v[i]; +// d.v[i]-=t*p;q=d.v[i];//q = d->v[i] -= t*p； +// for(int j = i; j < dim; j++ ){ +// NyARException.trap("未チェックパス"); +// a_array[i][j]-=p*(d.v[j] + q*vec.v[j]);//a->m[i*dim+j] -= p*(d->v[j]) + q*v[j]; +// } +// } +// } +// +// if( dim >= 2) { +// d.v[dim-2]=a_array[dim-2][dim-2];//d->v[dim-2] = a->m[(dim-2)*dim+(dim-2)]; +// e.v[dim-2+i_e_start]=a_array[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)]; +// } +// +// if( dim >= 1 ){ +// d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] = a->m[(dim-1)*dim+(dim-1)]; +// } +// +// for(int k = dim-1; k >= 0; k--) { +// vec=a.getRowVec(k);//v = a.getPointer(k*dim);//v = &(a->m[k*dim]); +// if( k < dim-2 ) { +// for(int i = k+1; i < dim; i++ ){ +// //wv1.clm = wv2.clm = dim-k-1; +// //wv1.v = &(v[k+1]); +// //wv2.v = &(a->m[i*dim+k+1]); +// vec2=a.getRowVec(i); +// +// t = vec.vecInnerproduct(vec2,k+1); +// for(int j = k+1; j < dim; j++ ){ +// NyARException.trap("未チェックパス"); +// a_array[i][j]-=t*vec.v[j];//a.subValue(i*dim+j,t*v.get(j));//a->m[i*dim+j] -= t * v[j]; +// } +// } +// } +// for(int i = 0; i < dim; i++ ){ +// vec.v[i]=0.0;//v.set(i,0.0);//v[i] = 0.0; +// } +// vec.v[k]=1;//v.set(k,1);//v[k] = 1; +// } +// } /** - * arVecTridiagonalize関数の代替品 - * a,d,e間で演算をしてる。何をどうしているかはさっぱりさっぱり - * @param a - * @param d - * @param e - * @param i_e_start - * 演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる) - * @return - * @throws NyARException + * 現在ラップしている配列を取り外して、新しい配列をラップします。 + * @param i_v + * @param i_clm */ - public static void vecTridiagonalize(NyARMat a, NyARVec d, NyARVec e,int i_e_start) throws NyARException + public void setNewArray(double[] i_array,int i_clm) { - NyARVec vec,vec2; - double[][] a_array=a.getArray(); - double s, t, p, q; - int dim; - - if(a.getClm()!=a.getRow()){ - throw new NyARException(); - } - if(a.getClm() != d.clm){ - throw new NyARException(); - } - if(a.getClm() != e.clm){ - throw new NyARException(); - } - dim = a.getClm(); - - for(int k = 0; k < dim-2; k++ ){ - vec=a.getRowVec(k); -// double[] vec_array=vec.getArray(); - NyARException.trap("未チェックパス"); - d.v[k]=vec.v[k];//d.set(k,v.get(k)); //d->v[k] = v[k]; - - //wv1.clm = dim-k-1; - //wv1.v = &(v[k+1]); - NyARException.trap("未チェックパス"); - e.v[k+i_e_start]=vecHousehold(vec,k+1);//e->v[k] = arVecHousehold(&wv1); - if(e.v[k+i_e_start]== 0.0 ){ - continue; - } - - for(int i = k+1; i < dim; i++ ){ - s = 0.0; - for(int j = k+1; j < i; j++ ) { - NyARException.trap("未チェックのパス"); - s += a_array[j][i] * vec.v[j];//s += a.get(j*dim+i) * v.get(j);//s += a->m[j*dim+i] * v[j]; - } - for(int j = i; j < dim; j++ ) { - NyARException.trap("未チェックのパス"); - s += a_array[i][j] * vec.v[j];//s += a.get(i*dim+j) * v.get(j);//s += a->m[i*dim+j] * v[j]; - } - NyARException.trap("未チェックのパス"); - d.v[i]=s;//d->v[i] = s; - } - - - //wv1.clm = wv2.clm = dim-k-1; - //wv1.v = &(v[k+1]); - //wv2.v = &(d->v[k+1]); - vec=a.getRowVec(k); -// vec_array=vec.getArray(); - NyARException.trap("未チェックパス"); - t = vecInnerproduct(vec,d,k+1)/ 2; - for(int i = dim-1; i > k; i-- ) { - NyARException.trap("未チェックパス"); - p = vec.v[i];//p = v.get(i);//p = v[i]; - d.v[i]-=t*p;q=d.v[i];//q = d->v[i] -= t*p； - for(int j = i; j < dim; j++ ){ - NyARException.trap("未チェックパス"); - a_array[i][j]-=p*(d.v[j] + q*vec.v[j]);//a->m[i*dim+j] -= p*(d->v[j]) + q*v[j]; - } - } - } - - if( dim >= 2) { - d.v[dim-2]=a_array[dim-2][dim-2];//d->v[dim-2] = a->m[(dim-2)*dim+(dim-2)]; - e.v[dim-2+i_e_start]=a_array[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)]; - } - - if( dim >= 1 ){ - d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] = a->m[(dim-1)*dim+(dim-1)]; - } - - for(int k = dim-1; k >= 0; k--) { - vec=a.getRowVec(k);//v = a.getPointer(k*dim);//v = &(a->m[k*dim]); - if( k < dim-2 ) { - for(int i = k+1; i < dim; i++ ){ - //wv1.clm = wv2.clm = dim-k-1; - //wv1.v = &(v[k+1]); - //wv2.v = &(a->m[i*dim+k+1]); - vec2=a.getRowVec(i); - - t = vecInnerproduct(vec,vec2,k+1); - for(int j = k+1; j < dim; j++ ){ - NyARException.trap("未チェックパス"); - a_array[i][j]-=t*vec.v[j];//a.subValue(i*dim+j,t*v.get(j));//a->m[i*dim+j] -= t * v[j]; - } - } - } - for(int i = 0; i < dim; i++ ){ - vec.v[i]=0.0;//v.set(i,0.0);//v[i] = 0.0; - } - vec.v[k]=1;//v.set(k,1);//v[k] = 1; - } + this.v=i_array; + this.clm=i_clm; } } diff --git a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_BlackWhite.java b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_BlackWhite.java index 26f3d24..7d8d371 100644 --- a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_BlackWhite.java +++ b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_BlackWhite.java @@ -51,7 +51,7 @@ public class NyARMatchPatt_BlackWhite implements ARMatchPatt{ { width=i_target_patt.getWidth(); height=i_target_patt.getHeight(); - short[][][] data=i_target_patt.getPatArray(); + int[][][] data=i_target_patt.getPatArray(); input=new int[height][width][3]; int sum = ave = 0; diff --git a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITHOUT_PCA.java b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITHOUT_PCA.java index 8f17c04..84df205 100644 --- a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITHOUT_PCA.java +++ b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITHOUT_PCA.java @@ -40,12 +40,11 @@ import jp.nyatla.nyartoolkit.core.*; * */ public class NyARMatchPatt_Color_WITHOUT_PCA implements ARMatchPatt{ - private int[][][] input; - private int ave; + private int[][][] input=new int[1][1][3]; private double datapow; - private int width; - private int height; + private int width =1; + private int height=1; private double cf=0; private int dir=0; public double getConfidence(){ @@ -54,29 +53,68 @@ public class NyARMatchPatt_Color_WITHOUT_PCA implements ARMatchPatt{ public int getDirection(){ return dir; } - public boolean setPatt(NyARColorPatt i_target_patt) throws NyARException + /** + * input配列サイズを必要に応じて再アロケートする。 + * + * @param i_width + * @param i_height + */ + private void reallocInputArray(int i_width,int i_height) { - width=i_target_patt.getWidth(); - height=i_target_patt.getHeight(); - short[][][] data=i_target_patt.getPatArray(); + if(this.input.length=0;i--){//for(int i=0;i=0;k--) {//for(int i2=0;i2l_ave += (255-data[i][i2][0])+(255-data[i][i2][1])+(255-data[i][i2][2]); + data_i_k=data_i[k]; + l_ave += 255*3-data_i_k[0]-data_i_k[1]-data_i_k[2]; } } - ave /= (height*width*3); - - for(int i=0;i=0;i--){//for(i=0;i=0;k--){//for(i2=0;i2 + //for(int i3=0;i3<3;i3++){ + // input[i][i2][i3] = (255-data[i][i2][i3]) - l_ave; + // sum += input[i][i2][i3]*input[i][i2][i3]; + //} + data_i_k =data_i[k]; + input_i_k=input_i[k]; + w_sum=(255-data_i_k[0]) - l_ave; + input_i_k[0]=w_sum; + sum += w_sum*w_sum; + + w_sum=(255-data_i_k[1]) - l_ave; + input_i_k[1]=w_sum; + sum += w_sum*w_sum; + + w_sum=(255-data_i_k[2]) - l_ave; + input_i_k[2]=w_sum; + sum+=w_sum*w_sum; + // } } datapow = Math.sqrt( (double)sum ); @@ -98,22 +136,35 @@ public class NyARMatchPatt_Color_WITHOUT_PCA implements ARMatchPatt{ double[] patpow=i_code.getPatPow(); int res= -1; double max=0.0; - for(int j = 0; j < 4; j++ ) { - int sum = 0; - for(int i=0;i max ){ - max = sum2; - res = j; - } - } - dir=res; - cf=max; + int[][][] pat_j,linput; + int[][] pat_j_i,input_i; + int[] pat_j_i_k,input_i_k; + int l_width=this.width; + int l_height=this.height; + linput=this.input; + for(int j = 0; j < 4; j++ ) { + int sum = 0; + pat_j=pat[j]; + for(int i=l_height-1;i>=0;i--){//for(int i=0;i=0;k--){ + pat_j_i_k=pat_j_i[k]; + input_i_k=input_i[k]; +// for(int i3=0;i3<3;i3++){ + sum += input_i_k[0]*pat_j_i_k[0];//sum += input[i][i2][i3]*pat[k][j][i][i2][i3]; + sum += input_i_k[1]*pat_j_i_k[1];//sum += input[i][i2][i3]*pat[k][j][i][i2][i3]; + sum += input_i_k[2]*pat_j_i_k[2];//sum += input[i][i2][i3]*pat[k][j][i][i2][i3]; +// } + } + } + double sum2 = sum / patpow[j] / datapow;//sum2 = sum / patpow[k][j] / datapow; + if( sum2 > max ){ + max = sum2; + res = j; + } + } + dir=res; + cf=max; } } \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITH_PCA.java b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITH_PCA.java index 17a38e7..11f9cce 100644 --- a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITH_PCA.java +++ b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITH_PCA.java @@ -63,7 +63,7 @@ public class NyARMatchPatt_Color_WITH_PCA implements ARMatchPatt{ { width=i_target_patt.getWidth(); height=i_target_patt.getHeight(); - short[][][] data=i_target_patt.getPatArray(); + int[][][] data=i_target_patt.getPatArray(); input=new int[height][width][3]; evec=new double[EVEC_MAX][height][width][3];//static double evec[EVEC_MAX][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3]; diff --git a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster.java b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster.java index 16f768f..ffda8b6 100644 --- a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster.java +++ b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster.java @@ -34,9 +34,28 @@ package jp.nyatla.nyartoolkit.core.raster; public interface NyARRaster{ //RGBの合計値を返す public int getPixelTotal(int i_x,int i_y); + /** + * 一行単位でi_row番目の合計値配列を計算して返す。 + * @param i_row + * @param o_line + * getWidth()の戻り値以上のサイズが必要。 + */ + public void getPixelTotalRowLine(int i_row,int[] o_line); public int getWidth(); public int getHeight(); - public void pickRgbArray(int i_x,int i_y,int[] i_rgb); + public void getPixel(int i_x,int i_y,int[] i_rgb); + /** + * 複数のピクセル値をi_rgbへ返します。 + * @param i_x + * xのインデックス配列 + * @param i_y + * yのインデックス配列 + * @param i_num + * 返すピクセル値の数 + * @param i_rgb + * ピクセル値を返すバッファ + */ + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb); } diff --git a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_BGRA.java b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_BGRA.java index 78797ce..01bcfc6 100644 --- a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_BGRA.java +++ b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_BGRA.java @@ -48,8 +48,18 @@ public class NyARRaster_BGRA implements NyARRaster //RGBの合計値を返す public int getPixelTotal(int i_x,int i_y) { - int bp=(i_x+i_y*width)*4; - return (ref_buf[bp] & 0xff)+(ref_buf[bp+1] & 0xff)+(ref_buf[bp+2] & 0xff); + int bp=(i_x+i_y*this.width)*4; + byte[] ref=this.ref_buf; + return (ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); + } + public void getPixelTotalRowLine(int i_row,int[] o_line) + { + final byte[] ref=this.ref_buf; + int bp=(i_row+1)*this.width*4-4; + for(int i=this.width-1;i>=0;i--){ + o_line[i]=(ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); + bp-=4; + } } public int getWidth() { @@ -59,12 +69,25 @@ public class NyARRaster_BGRA implements NyARRaster { return height; } - public void pickRgbArray(int i_x,int i_y,int[] i_rgb) + public void getPixel(int i_x,int i_y,int[] i_rgb) + { + byte[] ref=this.ref_buf; + int bp=(i_x+i_y*this.width)*4; + i_rgb[0]=(ref[bp+2] & 0xff);//R + i_rgb[1]=(ref[bp+1] & 0xff);//G + i_rgb[2]=(ref[bp+0] & 0xff);//B + } + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb) { - int bp=(i_x+i_y*width)*4; - i_rgb[0]=(ref_buf[bp+2] & 0xff);//R - i_rgb[1]=(ref_buf[bp+1] & 0xff);//G - i_rgb[2]=(ref_buf[bp+0] & 0xff);//B + int width=this.width; + byte[] ref=this.ref_buf; + int bp; + for(int i=i_num-1;i>=0;i--){ + bp=(i_x[i]+i_y[i]*width)*4; + o_rgb[i*3+0]=(ref[bp+2] & 0xff);//R + o_rgb[i*3+1]=(ref[bp+1] & 0xff);//G + o_rgb[i*3+2]=(ref[bp+0] & 0xff);//B + } } } diff --git a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_Blank.java b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_Blank.java index 4f9d851..1bea194 100644 --- a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_Blank.java +++ b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_Blank.java @@ -50,6 +50,12 @@ public class NyARRaster_Blank implements NyARRaster { return 0; } + public void getPixelTotalRowLine(int i_row,int[] o_line) + { + for(int i=this.width-1;i>=0;i--){ + o_line[i]=0; + } + } public int getWidth() { return width; @@ -58,10 +64,18 @@ public class NyARRaster_Blank implements NyARRaster { return height; } - public void pickRgbArray(int i_x,int i_y,int[] i_rgb) + public void getPixel(int i_x,int i_y,int[] i_rgb) { i_rgb[0]=0; i_rgb[1]=0; i_rgb[2]=0; } + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb) + { + for(int i=i_num-1;i>=0;i--){ + o_rgb[i*3+0]=0;//R + o_rgb[i*3+1]=0;//G + o_rgb[i*3+2]=0;//B + } + } } diff --git a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_RGB.java b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_RGB.java index e48040a..b223b9d 100644 --- a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_RGB.java +++ b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_RGB.java @@ -48,9 +48,19 @@ public class NyARRaster_RGB implements NyARRaster //RGBの合計値を返す public int getPixelTotal(int i_x,int i_y) { - int bp=(i_x+i_y*width)*3; - return (ref_buf[bp] & 0xff)+(ref_buf[bp+1] & 0xff)+(ref_buf[bp+2] & 0xff); + byte[] ref=this.ref_buf; + int bp=(i_x+i_y*this.width)*3; + return (ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); } + public void getPixelTotalRowLine(int i_row,int[] o_line) + { + final byte[] ref=this.ref_buf; + int bp=(i_row+1)*this.width*3-3; + for(int i=this.width-1;i>=0;i--){ + o_line[i]=(ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); + bp-=3; + } + } public int getWidth() { return width; @@ -59,12 +69,25 @@ public class NyARRaster_RGB implements NyARRaster { return height; } - public void pickRgbArray(int i_x,int i_y,int[] i_rgb) + public void getPixel(int i_x,int i_y,int[] i_rgb) + { + int bp=(i_x+i_y*this.width)*3; + byte[] ref=this.ref_buf; + i_rgb[0]=(ref[bp+0] & 0xff);//R + i_rgb[1]=(ref[bp+1] & 0xff);//G + i_rgb[2]=(ref[bp+2] & 0xff);//B + } + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb) { - int bp=(i_x+i_y*width)*3; - i_rgb[0]=(ref_buf[bp+0] & 0xff);//R - i_rgb[1]=(ref_buf[bp+1] & 0xff);//G - i_rgb[2]=(ref_buf[bp+2] & 0xff);//B + int width=this.width; + byte[] ref=this.ref_buf; + int bp; + for(int i=i_num-1;i>=0;i--){ + bp=(i_x[i]+i_y[i]*width)*3; + o_rgb[i*3+0]=(ref[bp+0] & 0xff);//R + o_rgb[i*3+1]=(ref[bp+1] & 0xff);//G + o_rgb[i*3+2]=(ref[bp+2] & 0xff);//B + } } } diff --git a/src/jp/nyatla/nyartoolkit/detector/NyARSingleDetectMarker.java b/src/jp/nyatla/nyartoolkit/detector/NyARSingleDetectMarker.java index a62e753..7e64092 100644 --- a/src/jp/nyatla/nyartoolkit/detector/NyARSingleDetectMarker.java +++ b/src/jp/nyatla/nyartoolkit/detector/NyARSingleDetectMarker.java @@ -40,9 +40,10 @@ import jp.nyatla.nyartoolkit.core.raster.*; * */ public class NyARSingleDetectMarker{ - private static final int AR_SQUARE_MAX=10; - private NyARParam param; + private static final int AR_SQUARE_MAX=100; + private NyARMatchPatt_Color_WITHOUT_PCA match_patt; private NyARDetectSquare square; + private final NyARSquareList square_list=new NyARSquareList(AR_SQUARE_MAX); private NyARCode code; protected NyARTransMat transmat; private double marker_width; @@ -51,18 +52,18 @@ public class NyARSingleDetectMarker{ private double detected_confidence; private NyARSquare detected_square; private NyARColorPatt patt; - public NyARSingleDetectMarker(NyARParam i_param,NyARCode i_code,double i_marker_width) + public NyARSingleDetectMarker(NyARParam i_param,NyARCode i_code,double i_marker_width) throws NyARException { - param=i_param; //解析オブジェクトを作る - square=new NyARDetectSquare(AR_SQUARE_MAX,i_param); - transmat=new NyARTransMat(param); + this.square=new NyARDetectSquare(i_param); + this.transmat=new NyARTransMat(i_param); //比較コードを保存 - code=i_code; - marker_width=i_marker_width; + this.code=i_code; + this.marker_width=i_marker_width; //評価パターンのホルダを作る - patt=new NyARColorPatt(code.getWidth(),code.getHeight()); - + this.patt=new NyARColorPatt_O3(code.getWidth(),code.getHeight()); + //評価器を作る。 + this.match_patt=new NyARMatchPatt_Color_WITHOUT_PCA(); } /** * i_imageにマーカー検出処理を実行して、結果を保持します。 @@ -75,49 +76,48 @@ public class NyARSingleDetectMarker{ public boolean detectMarkerLite(NyARRaster i_image,int i_thresh) throws NyARException { detected_square=null; + NyARSquareList l_square_list=this.square_list; //スクエアコードを探す - square.detectSquare(i_image, i_thresh); - int number_of_square=square.getSquareCount(); + square.detectSquare(i_image, i_thresh,l_square_list); + + int number_of_square=l_square_list.getSquareNum(); //コードは見つかった？ if(number_of_square<1){ return false; } //コードの一致度を調べる準備 - NyARSquare[] squares=square.getSquareArray(); +// NyARSquare[] squares=square.getSquareArray(); //評価基準になるパターンをイメージから切り出す - patt.pickFromRaster(i_image,squares[0].getMarker()); - - //パターンの評価オブジェクトを作る。 - NyARMatchPatt_Color_WITHOUT_PCA eva=new NyARMatchPatt_Color_WITHOUT_PCA(); + patt.pickFromRaster(i_image,l_square_list.getSquare(0)); //パターンを評価器にセット - if(!eva.setPatt(patt)){ + if(!this.match_patt.setPatt(patt)){ //計算に失敗した。 - return false; + throw new NyARException(); } //コードと比較する - eva.evaluate(code); + match_patt.evaluate(code); int square_index=0; - int direction=eva.getDirection(); - double confidence=eva.getConfidence(); + int direction=match_patt.getDirection(); + double confidence=match_patt.getConfidence(); for(int i=1;ic2){ continue; } //もっと一致するマーカーがあったぽい square_index=i; - direction=eva.getDirection(); + direction=match_patt.getDirection(); confidence=c2; } //マーカー情報を保存 - detected_square=squares[square_index]; + detected_square=l_square_list.getSquare(square_index); detected_direction=direction; detected_confidence=confidence; return true; diff --git a/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java b/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java index d1b84f3..1ba2ed6 100644 --- a/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java +++ b/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java @@ -36,7 +36,8 @@ import jp.nyatla.nyartoolkit.detector.*; /** * 320x240のBGRA32で記録されたRAWイメージから、１種類のパターンを認識し、 - * その変換行列を求めます。 + * その変換行列を1000回求め、それにかかったミリ秒時間を表示します。 + * * * @author R.iizuka * @@ -69,15 +70,19 @@ public class RawFileTest { //１パターンのみを追跡するクラスを作成 NyARSingleDetectMarker ar=new NyARSingleDetectMarker(ap,code,80.0); + ar.detectMarkerLite(ra,100); + ar.getTransmationMatrix(); //マーカーを検出 + double[][] tm; Date d2=new Date(); - for(int i=0;i<100;i++){ - ar.detectMarkerLite(ra,100); + for(int i=0;i<1000;i++){ //変換行列を取得 - double[][] tm=ar.getTransmationMatrix().getArray(); + ar.detectMarkerLite(ra,100); + ar.getTransmationMatrix(); } Date d=new Date(); + tm=null; System.out.println(d.getTime()-d2.getTime()); } public static void main(String[] args) diff --git a/src/jp/nyatla/util/IntValue.java b/src/jp/nyatla/util/IntValue.java index 7743171..be2d7da 100644 --- a/src/jp/nyatla/util/IntValue.java +++ b/src/jp/nyatla/util/IntValue.java @@ -1,17 +1,17 @@ package jp.nyatla.util; public class IntValue { - private int v; - public void set(int i_v){ - v=i_v; - } - public int get(){ - return v; - } - public void inc(){ - v++; - } - public void add(int i_v){ - v+=i_v; - } + public int value; +/* public void set(int i_v){ + v=i_v; + } + public int get(){ + return v; + } + public void inc(){ + v++; + } + public void add(int i_v){ + v+=i_v; + }*/ } -- 2.11.0