--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit JOGL sample program.\r
+ * --------------------------------------------------------------------------------\r
+ * The MIT License\r
+ * Copyright (c) 2008 nyatla\r
+ * airmail(at)ebony.plala.or.jp\r
+ * http://nyatla.jp/nyartoolkit/\r
+ * \r
+ * Permission is hereby granted, free of charge, to any person obtaining a copy\r
+ * of this software and associated documentation files (the "Software"), to deal\r
+ * in the Software without restriction, including without limitation the rights\r
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r
+ * copies of the Software, and to permit persons to whom the Software is\r
+ * furnished to do so, subject to the following conditions:\r
+ * The above copyright notice and this permission notice shall be included in\r
+ * all copies or substantial portions of the Software.\r
+ * \r
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\r
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\r
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\r
+ * THE SOFTWARE.\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.quadx2;\r
+\r
+import java.awt.event.WindowAdapter;\r
+import java.awt.event.WindowEvent;\r
+import java.awt.*;\r
+\r
+import javax.media.Buffer;\r
+\r
+import javax.media.opengl.GL;\r
+import javax.media.opengl.GLAutoDrawable;\r
+import javax.media.opengl.GLEventListener;\r
+import javax.media.opengl.GLCanvas;\r
+\r
+import com.sun.opengl.util.Animator;\r
+\r
+import jp.nyatla.nyartoolkit.core.NyARCode;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult;\r
+import jp.nyatla.nyartoolkit.jmf.utils.JmfCameraCapture;\r
+import jp.nyatla.nyartoolkit.jmf.utils.JmfCaptureListener;\r
+import jp.nyatla.nyartoolkit.jogl.utils.*;\r
+/**\r
+ * simpleLiteと同じようなテストプログラム\r
+ * 出来る限りARToolKitのサンプルと似せて作ってあります。\r
+ * 最も一致する"Hiro"マーカーを一つ選択して、その上に立方体を表示します。\r
+ * \r
+ */\r
+public class JavaSimpleLite_Quad implements GLEventListener, JmfCaptureListener\r
+{\r
+ private final String CARCODE_FILE = "../../Data/patt.hiro";\r
+\r
+ private final String PARAM_FILE = "../../Data/camera_para.dat";\r
+\r
+ private final static int SCREEN_X = 320;\r
+\r
+ private final static int SCREEN_Y = 240;\r
+\r
+ private Animator _animator;\r
+\r
+ private GLNyARRaster_RGB _cap_image;\r
+\r
+ private JmfCameraCapture _capture;\r
+\r
+ private GL _gl;\r
+\r
+ private NyARGLUtil _glnya;\r
+\r
+ //NyARToolkit関係\r
+ private NyARSingleDetectMarker_Quad _nya;\r
+\r
+ private NyARParam _ar_param;\r
+\r
+ /**\r
+ * 立方体を書く\r
+ *\r
+ */\r
+ void drawCube()\r
+ {\r
+ // Colour cube data.\r
+ int polyList = 0;\r
+ float fSize = 0.5f;//マーカーサイズに対して0.5倍なので、4cmの立方体\r
+ int f, i;\r
+ 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 } };\r
+ 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 } };\r
+ int cube_num_faces = 6;\r
+ 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 } };\r
+\r
+ if (polyList == 0) {\r
+ polyList = _gl.glGenLists(1);\r
+ _gl.glNewList(polyList, GL.GL_COMPILE);\r
+ _gl.glBegin(GL.GL_QUADS);\r
+ for (f = 0; f < cube_num_faces; f++)\r
+ for (i = 0; i < 4; i++) {\r
+ _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]);\r
+ _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);\r
+ }\r
+ _gl.glEnd();\r
+ _gl.glColor3f(0.0f, 0.0f, 0.0f);\r
+ for (f = 0; f < cube_num_faces; f++) {\r
+ _gl.glBegin(GL.GL_LINE_LOOP);\r
+ for (i = 0; i < 4; i++)\r
+ _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);\r
+ _gl.glEnd();\r
+ }\r
+ _gl.glEndList();\r
+ }\r
+\r
+ _gl.glPushMatrix(); // Save world coordinate system.\r
+ _gl.glTranslatef(0.0f, 0.0f, 0.5f); // Place base of cube on marker surface.\r
+ _gl.glRotatef(0.0f, 0.0f, 0.0f, 1.0f); // Rotate about z axis.\r
+ _gl.glDisable(GL.GL_LIGHTING); // Just use colours.\r
+ _gl.glCallList(polyList); // Draw the cube.\r
+ _gl.glPopMatrix(); // Restore world coordinate system.\r
+\r
+ }\r
+\r
+ public JavaSimpleLite_Quad()\r
+ {\r
+ Frame frame = new Frame("Java simpleLite with NyARToolkit");\r
+\r
+ // 3Dを描画するコンポーネント\r
+ GLCanvas canvas = new GLCanvas();\r
+ frame.add(canvas);\r
+ canvas.addGLEventListener(this);\r
+ frame.addWindowListener(new WindowAdapter() {\r
+ public void windowClosing(WindowEvent e)\r
+ {\r
+ System.exit(0);\r
+ }\r
+ });\r
+\r
+ frame.setVisible(true);\r
+ Insets ins = frame.getInsets();\r
+ frame.setSize(SCREEN_X + ins.left + ins.right, SCREEN_Y + ins.top + ins.bottom);\r
+ canvas.setBounds(ins.left, ins.top, SCREEN_X, SCREEN_Y);\r
+ }\r
+\r
+ public void init(GLAutoDrawable drawable)\r
+ {\r
+ _gl = drawable.getGL();\r
+ _gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f);\r
+ //NyARToolkitの準備\r
+ try {\r
+ //キャプチャの準備\r
+ _capture = new JmfCameraCapture(SCREEN_X, SCREEN_Y, 15f, JmfCameraCapture.PIXEL_FORMAT_RGB);\r
+ _capture.setCaptureListener(this);\r
+ //NyARToolkitの準備\r
+ _ar_param = new NyARParam();\r
+ NyARCode ar_code = new NyARCode(16, 16);\r
+ _ar_param.loadARParamFromFile(PARAM_FILE);\r
+ _ar_param.changeScreenSize(SCREEN_X, SCREEN_Y);\r
+ _nya = new NyARSingleDetectMarker_Quad(_ar_param, ar_code, 80.0);\r
+ _nya.setContinueMode(false);//ここをtrueにすると、transMatContinueモード（History計算）になります。\r
+ ar_code.loadARPattFromFile(CARCODE_FILE);\r
+ //NyARToolkit用の支援クラス\r
+ _glnya = new NyARGLUtil(_gl);\r
+ //GL対応のRGBラスタオブジェクト\r
+ _cap_image = new GLNyARRaster_RGB(_ar_param);\r
+ //キャプチャ開始\r
+ _capture.start();\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+ //カメラパラメータの計算\r
+ _glnya.toCameraFrustumRH(_ar_param,_camera_projection);\r
+ _animator = new Animator(drawable);\r
+\r
+ _animator.start();\r
+\r
+ }\r
+\r
+ public void reshape(GLAutoDrawable drawable, int x, int y, int width, int height)\r
+ {\r
+ _gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT);\r
+ _gl.glViewport(0, 0, width, height);\r
+\r
+ //視体積の設定\r
+ _gl.glMatrixMode(GL.GL_PROJECTION);\r
+ _gl.glLoadIdentity();\r
+ //見る位置\r
+ _gl.glMatrixMode(GL.GL_MODELVIEW);\r
+ _gl.glLoadIdentity();\r
+ }\r
+ private double[] _camera_projection=new double[16];\r
+ private NyARTransMatResult __display_transmat_result=new NyARTransMatResult();\r
+ private double[] __display_wk=new double[16];\r
+ public void display(GLAutoDrawable drawable)\r
+ {\r
+ NyARTransMatResult transmat_result=__display_transmat_result;\r
+ try {\r
+ if (!_cap_image.hasData()) {\r
+ return;\r
+ }\r
+ _gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT); // Clear the buffers for new frame. \r
+ //画像チェックしてマーカー探して、背景を書く\r
+ boolean is_marker_exist;\r
+ synchronized (_cap_image) {\r
+ is_marker_exist = _nya.detectMarkerLite(_cap_image, 110);\r
+ //背景を書く\r
+ _glnya.drawBackGround(_cap_image, 1.0);\r
+ }\r
+ //マーカーがあれば、立方体を描画\r
+ if (is_marker_exist) {\r
+ //マーカーの一致度を調査するならば、ここでnya.getConfidence()で一致度を調べて下さい。\r
+ // Projection transformation.\r
+ _gl.glMatrixMode(GL.GL_PROJECTION);\r
+ _gl.glLoadMatrixd(_camera_projection, 0);\r
+ _gl.glMatrixMode(GL.GL_MODELVIEW);\r
+ // Viewing transformation.\r
+ _gl.glLoadIdentity();\r
+ //変換行列を取得\r
+ _nya.getTransmationMatrix(transmat_result);\r
+ //変換行列をOpenGL形式に変換\r
+ _glnya.toCameraViewRH(transmat_result, __display_wk);\r
+ _gl.glLoadMatrixd(__display_wk, 0);\r
+\r
+ // All other lighting and geometry goes here.\r
+ drawCube();\r
+ }\r
+ Thread.sleep(1);//タスク実行権限を一旦渡す\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+\r
+ }\r
+ public void onUpdateBuffer(Buffer i_buffer)\r
+ {\r
+ try {\r
+ synchronized (_cap_image) {\r
+ _cap_image.setBuffer(i_buffer, true);\r
+ }\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+ }\r
+\r
+ public void displayChanged(GLAutoDrawable drawable, boolean modeChanged, boolean deviceChanged)\r
+ {\r
+ }\r
+\r
+ public static void main(String[] args)\r
+ {\r
+ new JavaSimpleLite_Quad();\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.quadx2;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.raster.*;\r
+import jp.nyatla.nyartoolkit.core.raster.rgb.INyARRgbRaster;\r
+import jp.nyatla.nyartoolkit.core.rasterreader.INyARBufferReader;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.rasterfilter.rgb2bin.*;\r
+\r
+/**\r
+ * 1/4のサイズの画像に変換しながら閾値判定する関数\r
+ * \r
+ */\r
+public class NyARRasterFilter_ARTTh_Quad implements INyARRasterFilter_RgbToBin\r
+{\r
+ private int _threshold;\r
+\r
+ public NyARRasterFilter_ARTTh_Quad(int i_threshold)\r
+ {\r
+ this._threshold = i_threshold;\r
+ }\r
+ public void setThreshold(int i_threshold)\r
+ {\r
+ this._threshold = i_threshold;\r
+ }\r
+\r
+ public void doFilter(INyARRgbRaster i_input, NyARBinRaster i_output) throws NyARException\r
+ {\r
+ INyARBufferReader in_buffer_reader=i_input.getBufferReader(); \r
+ INyARBufferReader out_buffer_reader=i_output.getBufferReader();\r
+ int in_buf_type=in_buffer_reader.getBufferType();\r
+\r
+ NyARIntSize size = i_output.getSize();\r
+ assert (out_buffer_reader.isEqualBufferType(INyARBufferReader.BUFFERFORMAT_INT2D_BIN_8));\r
+ assert (checkInputType(in_buf_type)==true); \r
+ assert (i_input.getSize().isEqualSize(size.w*2,size.h*2) == true);\r
+\r
+ int[][] out_buf = (int[][]) out_buffer_reader.getBuffer();\r
+ byte[] in_buf = (byte[]) in_buffer_reader.getBuffer();\r
+\r
+ switch (in_buffer_reader.getBufferType()) {\r
+ case INyARBufferReader.BUFFERFORMAT_BYTE1D_B8G8R8_24:\r
+ case INyARBufferReader.BUFFERFORMAT_BYTE1D_R8G8B8_24:\r
+ convert24BitRgb(in_buf, out_buf, size);\r
+ break;\r
+// case INyARBufferReader.BUFFERFORMAT_BYTE1D_B8G8R8X8_32:\r
+// convert32BitRgbx(in_buf, out_buf, size);\r
+// break;\r
+ default:\r
+ throw new NyARException();\r
+ }\r
+ return;\r
+ }\r
+\r
+ private void convert24BitRgb(byte[] i_in, int[][] i_out, NyARIntSize i_size)\r
+ {\r
+ final int size_w=i_size.w*2;\r
+ final int x_mod_end= size_w-(size_w%8);\r
+ final int th=this._threshold*3;\r
+ int bp =(size_w*i_size.h*2-1)*3; \r
+ int w;\r
+ int x; \r
+ for (int y =i_size.h-1; y>=0 ; y--){\r
+ //端数分\r
+ final int[] row_ptr=i_out[y];\r
+ for (x = i_size.w-1;x>=x_mod_end;x--) {\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x]=w<=th?0:1;\r
+ bp -= 6;\r
+ }\r
+ //タイリング \r
+ for (;x>=0;x-=8) {\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x]=w<=th?0:1;\r
+ bp -= 6;\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-1]=w<=th?0:1;\r
+ bp -= 6;\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-2]=w<=th?0:1;\r
+ bp -= 6;\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-3]=w<=th?0:1;\r
+ bp -= 6;\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-4]=w<=th?0:1;\r
+ bp -= 6;\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-5]=w<=th?0:1;\r
+ bp -= 6;\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-6]=w<=th?0:1;\r
+ bp -= 6;\r
+ w=((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-7]=w<=th?0:1;\r
+ bp -= 6;\r
+ }\r
+ //1行飛ばし\r
+ bp-=size_w*3;\r
+ }\r
+ return;\r
+ }\r
+ private void convert32BitRgbx(byte[] i_in, int[][] i_out, NyARIntSize i_size)\r
+ {\r
+ final int size_w=i_size.w;\r
+ final int x_mod_end= size_w-(size_w%8);\r
+ final int th=this._threshold*3;\r
+ int bp =(size_w*i_size.h-1)*4;\r
+ int w;\r
+ int x;\r
+ for (int y =i_size.h-1; y>=0 ; y--){\r
+ final int[] row_ptr=i_out[y];\r
+\r
+ //端数分\r
+ for (x = size_w-1;x>=x_mod_end;x--) {\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x]=w<=th?0:1;\r
+ bp -= 4;\r
+ }\r
+ //タイリング\r
+ for (;x>=0;x-=8) {\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x]=w<=th?0:1;\r
+ bp -= 4;\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-1]=w<=th?0:1;\r
+ bp -= 4;\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-2]=w<=th?0:1;\r
+ bp -= 4;\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-3]=w<=th?0:1;\r
+ bp -= 4;\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-4]=w<=th?0:1;\r
+ bp -= 4;\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-5]=w<=th?0:1;\r
+ bp -= 4;\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-6]=w<=th?0:1;\r
+ bp -= 4;\r
+ w= ((i_in[bp] & 0xff) + (i_in[bp + 1] & 0xff) + (i_in[bp + 2] & 0xff));\r
+ row_ptr[x-7]=w<=th?0:1;\r
+ bp -= 4;\r
+ } \r
+ }\r
+ return;\r
+ }\r
+ \r
+ private boolean checkInputType(int i_input_type) throws NyARException\r
+ {\r
+ switch(i_input_type){\r
+ case INyARBufferReader.BUFFERFORMAT_BYTE1D_B8G8R8_24:\r
+ case INyARBufferReader.BUFFERFORMAT_BYTE1D_R8G8B8_24:\r
+// case INyARBufferReader.BUFFERFORMAT_BYTE1D_B8G8R8X8_32:\r
+// case INyARBufferReader.BUFFERFORMAT_BYTE1D_R5G6B5_16LE:\r
+ return true;\r
+ default:\r
+ return false;\r
+ }\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.quadx2;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.*;\r
+import jp.nyatla.nyartoolkit.core.match.*;\r
+import jp.nyatla.nyartoolkit.core.param.NyARParam;\r
+import jp.nyatla.nyartoolkit.core.pickup.*;\r
+import jp.nyatla.nyartoolkit.core.raster.rgb.*;\r
+import jp.nyatla.nyartoolkit.core.raster.*;\r
+import jp.nyatla.nyartoolkit.core.transmat.*;\r
+import jp.nyatla.nyartoolkit.core.types.NyARIntSize;\r
+import jp.nyatla.nyartoolkit.core.rasterfilter.rgb2bin.NyARRasterFilter_ARToolkitThreshold;\r
+import jp.nyatla.nyartoolkit.sandbox.x2.NyARSquareDetector_X2;\r
+import jp.nyatla.nyartoolkit.sandbox.x2.NyARTransMat_X2;\r
+\r
+\r
+/**\r
+ * 画像からARCodeに最も一致するマーカーを1個検出し、その変換行列を計算するクラスです。\r
+ * \r
+ */\r
+public class NyARSingleDetectMarker_Quad\r
+{\r
+ private static final int AR_SQUARE_MAX = 100;\r
+\r
+ private boolean _is_continue = false;\r
+ private NyARMatchPatt_Color_WITHOUT_PCA _match_patt;\r
+ private INyARSquareDetector _square_detect;\r
+\r
+ private final NyARSquareStack _square_list = new NyARSquareStack(AR_SQUARE_MAX);\r
+\r
+ private NyARCode _code;\r
+\r
+ protected INyARTransMat _transmat;\r
+\r
+ private double _marker_width;\r
+\r
+ // 検出結果の保存用\r
+ private int _detected_direction;\r
+\r
+ private double _detected_confidence;\r
+\r
+ private NyARSquare _detected_square;\r
+\r
+ private INyARColorPatt _patt;\r
+\r
+ /**\r
+ * 検出するARCodeとカメラパラメータから、1個のARCodeを検出するNyARSingleDetectMarkerインスタンスを作ります。\r
+ * \r
+ * @param i_param\r
+ * カメラパラメータを指定します。\r
+ * @param i_code\r
+ * 検出するARCodeを指定します。\r
+ * @param i_marker_width\r
+ * ARコードの物理サイズを、ミリメートルで指定します。\r
+ * @throws NyARException\r
+ */\r
+ public NyARSingleDetectMarker_Quad(NyARParam i_param, NyARCode i_code, double i_marker_width) throws NyARException\r
+ {\r
+ final NyARIntSize scr_size=i_param.getScreenSize(); \r
+ // 解析オブジェクトを作る\r
+ this._square_detect = new NyARSquareDetector_Quad(i_param.getDistortionFactor(),scr_size);\r
+ this._transmat = new NyARTransMat_X2(i_param);\r
+ // 比較コードを保存\r
+ this._code = i_code;\r
+ this._marker_width = i_marker_width;\r
+ // 評価パターンのホルダを作る\r
+ this._patt = new NyARColorPatt_O3(_code.getWidth(), _code.getHeight());\r
+ // 評価器を作る。\r
+ this._match_patt = new NyARMatchPatt_Color_WITHOUT_PCA();\r
+ //２値画像バッファを作る\r
+ this._bin_raster=new NyARBinRaster(scr_size.w/2,scr_size.h/2);\r
+ return;\r
+ }\r
+\r
+ private NyARBinRaster _bin_raster;\r
+// private NyARRasterFilter_ARToolkitThreshold _tobin_filter=new NyARRasterFilter_ARToolkitThreshold(100);\r
+ private NyARRasterFilter_ARTTh_Quad _tobin_filter=new NyARRasterFilter_ARTTh_Quad(100);\r
+\r
+ /**\r
+ * i_imageにマーカー検出処理を実行し、結果を記録します。\r
+ * \r
+ * @param i_raster\r
+ * マーカーを検出するイメージを指定します。イメージサイズは、カメラパラメータ\r
+ * と一致していなければなりません。\r
+ * @return マーカーが検出できたかを真偽値で返します。\r
+ * @throws NyARException\r
+ */\r
+ public boolean detectMarkerLite(INyARRgbRaster i_raster,int i_threshold) throws NyARException\r
+ {\r
+ //サイズチェック\r
+ if(!this._bin_raster.getSize().isEqualSize(i_raster.getSize().w/2,i_raster.getSize().h/2)){\r
+ throw new NyARException();\r
+ }\r
+\r
+ //ラスタを(1/4の画像の)２値イメージに変換する.\r
+ this._tobin_filter.setThreshold(i_threshold);\r
+ this._tobin_filter.doFilter(i_raster,this._bin_raster);\r
+ \r
+ \r
+ this._detected_square = null;\r
+ NyARSquareStack l_square_list = this._square_list;\r
+ // スクエアコードを探す\r
+ this._square_detect.detectMarker(this._bin_raster, l_square_list);\r
+\r
+\r
+ int number_of_square = l_square_list.getLength();\r
+ // コードは見つかった？\r
+ if (number_of_square < 1) {\r
+ return false;\r
+ }\r
+\r
+ // 評価基準になるパターンをイメージから切り出す\r
+ if (!this._patt.pickFromRaster(i_raster, (NyARSquare)l_square_list.getItem(0))) {\r
+ // パターンの切り出しに失敗\r
+ return false;\r
+ }\r
+ // パターンを評価器にセット\r
+ if (!this._match_patt.setPatt(this._patt)) {\r
+ // 計算に失敗した。\r
+ throw new NyARException();\r
+ }\r
+ // コードと比較する\r
+ this._match_patt.evaluate(this._code);\r
+ int square_index = 0;\r
+ int direction = this._match_patt.getDirection();\r
+ double confidence = this._match_patt.getConfidence();\r
+ for (int i = 1; i < number_of_square; i++) {\r
+ // 次のパターンを取得\r
+ this._patt.pickFromRaster(i_raster, (NyARSquare)l_square_list.getItem(i));\r
+ // 評価器にセットする。\r
+ this._match_patt.setPatt(this._patt);\r
+ // コードと比較する\r
+ this._match_patt.evaluate(this._code);\r
+ double c2 = this._match_patt.getConfidence();\r
+ if (confidence > c2) {\r
+ continue;\r
+ }\r
+ // もっと一致するマーカーがあったぽい\r
+ square_index = i;\r
+ direction = this._match_patt.getDirection();\r
+ confidence = c2;\r
+ }\r
+ // マーカー情報を保存\r
+ this._detected_square = (NyARSquare)l_square_list.getItem(square_index);\r
+ this._detected_direction = direction;\r
+ this._detected_confidence = confidence;\r
+ return true;\r
+ }\r
+\r
+ /**\r
+ * 検出したマーカーの変換行列を計算して、o_resultへ値を返します。\r
+ * 直前に実行したdetectMarkerLiteが成功していないと使えません。\r
+ * \r
+ * @param o_result\r
+ * 変換行列を受け取るオブジェクトを指定します。\r
+ * @throws NyARException\r
+ */\r
+ public void getTransmationMatrix(NyARTransMatResult o_result) throws NyARException\r
+ {\r
+ // 一番一致したマーカーの位置とかその辺を計算\r
+ if (this._is_continue) {\r
+ this._transmat.transMatContinue(this._detected_square,this._detected_direction,this._marker_width, o_result);\r
+ } else {\r
+ this._transmat.transMat(this._detected_square,this._detected_direction,this._marker_width, o_result);\r
+ }\r
+ return;\r
+ }\r
+\r
+ /**\r
+ * 検出したマーカーの一致度を返します。\r
+ * \r
+ * @return マーカーの一致度を返します。0～1までの値をとります。 一致度が低い場合には、誤認識の可能性が高くなります。\r
+ * @throws NyARException\r
+ */\r
+ public double getConfidence()\r
+ {\r
+ return this._detected_confidence;\r
+ }\r
+\r
+ /**\r
+ * 検出したマーカーの方位を返します。\r
+ * \r
+ * @return 0,1,2,3の何れかを返します。\r
+ */\r
+ public int getDirection()\r
+ {\r
+ return this._detected_direction;\r
+ }\r
+\r
+ /**\r
+ * getTransmationMatrixの計算モードを設定します。 初期値はTRUEです。\r
+ * \r
+ * @param i_is_continue\r
+ * TRUEなら、transMatCont互換の計算をします。 FALSEなら、transMat互換の計算をします。\r
+ */\r
+ public void setContinueMode(boolean i_is_continue)\r
+ {\r
+ this._is_continue = i_is_continue;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.quadx2;\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.labeling.*;\r
+import jp.nyatla.nyartoolkit.core.raster.*;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core.pca2d.*;\r
+import jp.nyatla.nyartoolkit.core2.pca2d.NyARPca2d_SamplingPCA;\r
+import jp.nyatla.nyartoolkit.core.*;\r
+import jp.nyatla.nyartoolkit.sandbox.x2.*;\r
+\r
+\r
+/**\r
+ * 1/4に解像度を落して解析するNyARSquareDetector_X2\r
+ * 与えるBinRasterが既に1/4のサイズになっていないといけないことに注意\r
+ */\r
+public class NyARSquareDetector_Quad implements INyARSquareDetector\r
+{\r
+ private static final double VERTEX_FACTOR = 1.0;// 線検出のファクタ\r
+\r
+ private static final int AR_AREA_MAX = 25000;// #define AR_AREA_MAX 100000\r
+\r
+ private static final int AR_AREA_MIN = 20;// #define AR_AREA_MIN 70\r
+ private final int _width;\r
+ private final int _height;\r
+\r
+ private final INyARLabeling _labeling;\r
+\r
+ private final NyARLabelingImage _limage;\r
+\r
+ private final OverlapChecker _overlap_checker = new OverlapChecker();\r
+ private final NyARCameraDistortionFactor _dist_factor=new NyARCameraDistortionFactor();\r
+\r
+ /**\r
+ * 最大i_squre_max個のマーカーを検出するクラスを作成する。\r
+ * \r
+ * @param i_param\r
+ */\r
+ public NyARSquareDetector_Quad(NyARCameraDistortionFactor i_dist_factor_ref,NyARIntSize i_size) throws NyARException\r
+ {\r
+ this._width = i_size.w/2;\r
+ this._height = i_size.h/2;\r
+ this._labeling = new NyARLabeling_ARToolKit_X2();\r
+ this._limage = new NyARLabelingImage(this._width, this._height);\r
+ this._labeling.attachDestination(this._limage);\r
+\r
+ // 輪郭の最大長は画面に映りうる最大の長方形サイズ。\r
+ int number_of_coord = (this._width + this._height) * 2;\r
+\r
+ // 輪郭バッファは頂点変換をするので、輪郭バッファの２倍取る。\r
+ this._max_coord = number_of_coord;\r
+ this._xcoord = new int[number_of_coord * 2];\r
+ this._ycoord = new int[number_of_coord * 2];\r
+ //PCA\r
+ this._pca=new NyARPca2d_SamplingPCA(20);\r
+ //内部的に1/4のサイズで取り扱う\r
+ this._dist_factor.copyFrom(i_dist_factor_ref);\r
+ this._dist_factor.changeScale(0.5);\r
+ \r
+ }\r
+\r
+ private final int _max_coord;\r
+ private final int[] _xcoord;\r
+ private final int[] _ycoord;\r
+\r
+ private void normalizeCoord(int[] i_coord_x, int[] i_coord_y, int i_index, int i_coord_num)\r
+ {\r
+ // vertex1を境界にして、後方に配列を連結\r
+ System.arraycopy(i_coord_x, 1, i_coord_x, i_coord_num, i_index);\r
+ System.arraycopy(i_coord_y, 1, i_coord_y, i_coord_num, i_index);\r
+ }\r
+\r
+ private final int[] __detectMarker_mkvertex = new int[5];\r
+\r
+ /**\r
+ * arDetectMarker2を基にした関数\r
+ * この関数はNyARSquare要素のうち、directionを除くパラメータを取得して返します。\r
+ * directionの確定は行いません。\r
+ * @param i_raster\r
+ * 解析する２値ラスタイメージを指定します。\r
+ * @param o_square_stack\r
+ * 抽出した正方形候補を格納するリスト\r
+ * @throws NyARException\r
+ */\r
+ public final void detectMarker(NyARBinRaster i_raster, NyARSquareStack o_square_stack) throws NyARException\r
+ {\r
+ final INyARLabeling labeling_proc = this._labeling;\r
+ final NyARLabelingImage limage = this._limage;\r
+\r
+ // 初期化\r
+\r
+ // マーカーホルダをリセット\r
+ o_square_stack.clear();\r
+\r
+ // ラベリング\r
+ labeling_proc.labeling(i_raster);\r
+\r
+ // ラベル数が0ならここまで\r
+ final int label_num = limage.getLabelStack().getLength();\r
+ if (label_num < 1) {\r
+ return;\r
+ }\r
+\r
+ final NyARLabelingLabelStack stack = limage.getLabelStack();\r
+ final NyARLabelingLabel[] labels = (NyARLabelingLabel[])stack.getArray();\r
+ \r
+ \r
+ // ラベルを大きい順に整列\r
+ stack.sortByArea();\r
+\r
+ // デカいラベルを読み飛ばし\r
+ int i;\r
+ for (i = 0; i < label_num; i++) {\r
+ // 検査対象内のラベルサイズになるまで無視\r
+ if (labels[i].area <= AR_AREA_MAX) {\r
+ break;\r
+ }\r
+ }\r
+\r
+ final int xsize = this._width;\r
+ final int ysize = this._height;\r
+ final int[] xcoord = this._xcoord;\r
+ final int[] ycoord = this._ycoord;\r
+ final int coord_max = this._max_coord;\r
+ final int[] mkvertex = this.__detectMarker_mkvertex;\r
+ final OverlapChecker overlap = this._overlap_checker;\r
+ int coord_num;\r
+ int label_area;\r
+ NyARLabelingLabel label_pt;\r
+\r
+ //重なりチェッカの最大数を設定\r
+ overlap.reset(label_num);\r
+\r
+ for (; i < label_num; i++) {\r
+ label_pt = labels[i];\r
+ label_area = label_pt.area;\r
+ // 検査対象サイズよりも小さくなったら終了\r
+ if (label_area < AR_AREA_MIN) {\r
+ break;\r
+ }\r
+ // クリップ領域が画面の枠に接していれば除外\r
+ if (label_pt.clip_l == 1 || label_pt.clip_r == xsize - 2) {// if(wclip[i*4+0] == 1 || wclip[i*4+1] ==xsize-2){\r
+ continue;\r
+ }\r
+ if (label_pt.clip_t == 1 || label_pt.clip_b == ysize - 2) {// if( wclip[i*4+2] == 1 || wclip[i*4+3] ==ysize-2){\r
+ continue;\r
+ }\r
+ // 既に検出された矩形との重なりを確認\r
+ if (!overlap.check(label_pt)) {\r
+ // 重なっているようだ。\r
+ continue;\r
+ }\r
+\r
+ // 輪郭を取得\r
+ coord_num = limage.getContour(i, coord_max, xcoord, ycoord);\r
+ if (coord_num == coord_max) {\r
+ // 輪郭が大きすぎる。\r
+ continue;\r
+ }\r
+ //頂点候補のインデクスを取得\r
+ final int vertex1 = scanVertex(xcoord, ycoord, coord_num);\r
+\r
+ // 頂点候補(vertex1)を先頭に並べなおした配列を作成する。\r
+ normalizeCoord(xcoord, ycoord, vertex1, coord_num);\r
+\r
+ // 領域を準備する。\r
+ NyARSquare square_ptr = (NyARSquare)o_square_stack.prePush();\r
+\r
+ // 頂点情報を取得\r
+ if (!getSquareVertex(xcoord, ycoord, vertex1, coord_num, label_area, mkvertex)) {\r
+ o_square_stack.pop();// 頂点の取得が出来なかったので破棄\r
+ continue;\r
+ }\r
+ // マーカーを検出\r
+ if (!getSquareLine(mkvertex, xcoord, ycoord, square_ptr)) {\r
+ // 矩形が成立しなかった。\r
+ o_square_stack.pop();\r
+ continue;\r
+ }\r
+ // 検出済の矩形の属したラベルを重なりチェックに追加する。\r
+ overlap.push(label_pt);\r
+ } \r
+ return;\r
+ }\r
+\r
+ /**\r
+ * 辺からの対角線が最長になる点を対角線候補として返す。\r
+ * \r
+ * @param i_xcoord\r
+ * @param i_ycoord\r
+ * @param i_coord_num\r
+ * @return\r
+ */\r
+ private int scanVertex(int[] i_xcoord, int[] i_ycoord, int i_coord_num)\r
+ {\r
+ final int sx = i_xcoord[0];\r
+ final int sy = i_ycoord[0];\r
+ int d = 0;\r
+ int w, x, y;\r
+ int ret = 0;\r
+ for (int i = 1; i < i_coord_num; i++) {\r
+ x = i_xcoord[i] - sx;\r
+ y = i_ycoord[i] - sy;\r
+ w = x * x + y * y;\r
+ if (w > d) {\r
+ d = w;\r
+ ret = i;\r
+ }\r
+ // ここでうまく終了条件入れられないかな。\r
+ }\r
+ return ret;\r
+ }\r
+\r
+ private final NyARFixedFloatVertexCounter __getSquareVertex_wv1 = new NyARFixedFloatVertexCounter();\r
+\r
+ private final NyARFixedFloatVertexCounter __getSquareVertex_wv2 = new NyARFixedFloatVertexCounter();\r
+\r
+ /**\r
+ * static int arDetectMarker2_check_square( int area, ARMarkerInfo2 *marker_info2, double factor ) 関数の代替関数 OPTIMIZED STEP [450->415] o_squareに頂点情報をセットします。\r
+ * \r
+ * @param i_x_coord\r
+ * @param i_y_coord\r
+ * @param i_vertex1_index\r
+ * @param i_coord_num\r
+ * @param i_area\r
+ * @param o_vertex\r
+ * 要素数はint[4]である事\r
+ * @return\r
+ */\r
+ private boolean getSquareVertex(int[] i_x_coord, int[] i_y_coord, int i_vertex1_index, int i_coord_num, int i_area, int[] o_vertex)\r
+ {\r
+ final NyARFixedFloatVertexCounter wv1 = this.__getSquareVertex_wv1;\r
+ final NyARFixedFloatVertexCounter wv2 = this.__getSquareVertex_wv2;\r
+ final int end_of_coord = i_vertex1_index + i_coord_num - 1;\r
+ final int sx = i_x_coord[i_vertex1_index];// sx = marker_info2->x_coord[0];\r
+ final int sy = i_y_coord[i_vertex1_index];// sy = marker_info2->y_coord[0];\r
+ int dmax = 0;\r
+ int v1 = i_vertex1_index;\r
+ for (int i = 1 + i_vertex1_index; i < end_of_coord; i++) {// for(i=1;i<marker_info2->coord_num-1;i++)\r
+ // {\r
+ final int d = (i_x_coord[i] - sx) * (i_x_coord[i] - sx) + (i_y_coord[i] - sy) * (i_y_coord[i] - sy);\r
+ if (d > dmax) {\r
+ dmax = d;\r
+ v1 = i;\r
+ }\r
+ }\r
+ final double thresh = (i_area / 0.75) * 0.01 * VERTEX_FACTOR;\r
+\r
+ o_vertex[0] = i_vertex1_index;\r
+\r
+ if (!wv1.getVertex(i_x_coord, i_y_coord, i_vertex1_index, v1, thresh)) { // if(get_vertex(marker_info2->x_coord,marker_info2->y_coord,0,v1,thresh,wv1,&wvnum1)<\r
+ // 0 ) {\r
+ return false;\r
+ }\r
+ if (!wv2.getVertex(i_x_coord, i_y_coord, v1, end_of_coord, thresh)) {// if(get_vertex(marker_info2->x_coord,marker_info2->y_coord,v1,marker_info2->coord_num-1,thresh,wv2,&wvnum2)\r
+ // < 0) {\r
+ return false;\r
+ }\r
+\r
+ int v2;\r
+ if (wv1.number_of_vertex == 1 && wv2.number_of_vertex == 1) {// if(wvnum1 == 1 && wvnum2== 1) {\r
+ o_vertex[1] = wv1.vertex[0];\r
+ o_vertex[2] = v1;\r
+ o_vertex[3] = wv2.vertex[0];\r
+ } else if (wv1.number_of_vertex > 1 && wv2.number_of_vertex == 0) {// }else if( wvnum1 > 1 && wvnum2== 0) {\r
+ //頂点位置を、起点から対角点の間の1/2にあると予想して、検索する。\r
+ v2 = (v1-i_vertex1_index)/2+i_vertex1_index;\r
+ if (!wv1.getVertex(i_x_coord, i_y_coord, i_vertex1_index, v2, thresh)) {\r
+ return false;\r
+ }\r
+ if (!wv2.getVertex(i_x_coord, i_y_coord, v2, v1, thresh)) {\r
+ return false;\r
+ }\r
+ if (wv1.number_of_vertex == 1 && wv2.number_of_vertex == 1) {\r
+ o_vertex[1] = wv1.vertex[0];\r
+ o_vertex[2] = wv2.vertex[0];\r
+ o_vertex[3] = v1;\r
+ } else {\r
+ return false;\r
+ }\r
+ } else if (wv1.number_of_vertex == 0 && wv2.number_of_vertex > 1) {\r
+ //v2 = (v1-i_vertex1_index+ end_of_coord-i_vertex1_index) / 2+i_vertex1_index;\r
+ v2 = (v1+ end_of_coord)/2;\r
+\r
+ if (!wv1.getVertex(i_x_coord, i_y_coord, v1, v2, thresh)) {\r
+ return false;\r
+ }\r
+ if (!wv2.getVertex(i_x_coord, i_y_coord, v2, end_of_coord, thresh)) {\r
+ return false;\r
+ }\r
+ if (wv1.number_of_vertex == 1 && wv2.number_of_vertex == 1) {\r
+ o_vertex[1] = v1;\r
+ o_vertex[2] = wv1.vertex[0];\r
+ o_vertex[3] = wv2.vertex[0];\r
+ } else {\r
+ return false;\r
+ }\r
+ } else {\r
+ return false;\r
+ }\r
+ o_vertex[4] = end_of_coord;\r
+ return true;\r
+ }\r
+\r
+ private final INyARPca2d _pca;\r
+ private final NyARDoubleMatrix22 __getSquareLine_evec=new NyARDoubleMatrix22();\r
+ private final NyARDoublePoint2d __getSquareLine_mean=new NyARDoublePoint2d();\r
+ private final NyARDoublePoint2d __getSquareLine_ev=new NyARDoublePoint2d();\r
+ /**\r
+ * 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\r
+ * coord_num,int vertex[], double line[4][3], double v[4][2], double *dist_factor) の２関数の合成品です。 マーカーのvertex,lineを計算して、結果をo_squareに保管します。\r
+ * Optimize:STEP[424->391]\r
+ * \r
+ * @param i_cparam\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ private boolean getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square) throws NyARException\r
+ {\r
+ final NyARLinear[] l_line = o_square.line;\r
+ final NyARCameraDistortionFactor dist_factor=this._dist_factor; \r
+ final NyARDoubleMatrix22 evec=this.__getSquareLine_evec;\r
+ final NyARDoublePoint2d mean=this.__getSquareLine_mean;\r
+ final NyARDoublePoint2d ev=this.__getSquareLine_ev;\r
+ \r
+ \r
+ for (int i = 0; i < 4; i++) {\r
+ final double w1 = (double) (i_mkvertex[i + 1] - i_mkvertex[i] + 1) * 0.05 + 0.5;\r
+ final int st = (int) (i_mkvertex[i] + w1);\r
+ final int ed = (int) (i_mkvertex[i + 1] - w1);\r
+ final int n = ed - st + 1;\r
+ if (n < 2) {\r
+ // nが2以下でmatrix.PCAを計算することはできないので、エラー\r
+ return false;\r
+ }\r
+ //主成分分析する。\r
+ this._pca.pcaWithDistortionFactor(i_xcoord, i_ycoord, st, n,dist_factor, evec, ev,mean);\r
+ final NyARLinear l_line_i = l_line[i];\r
+ l_line_i.run = evec.m01;// line[i][0] = evec->m[1];\r
+ l_line_i.rise = -evec.m00;// line[i][1] = -evec->m[0];\r
+ l_line_i.intercept = -(l_line_i.run * mean.x + l_line_i.rise * mean.y)*2;// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);\r
+ }\r
+\r
+ final NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex;\r
+ final NyARIntPoint[] l_imvertex = o_square.imvertex;\r
+ for (int i = 0; i < 4; i++) {\r
+ final NyARLinear l_line_i = l_line[i];\r
+ final NyARLinear l_line_2 = l_line[(i + 3) % 4];\r
+ final double w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise;\r
+ if (w1 == 0.0) {\r
+ return false;\r
+ }\r
+ l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1;\r
+ l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1;\r
+ // 頂点インデクスから頂点座標を得て保存\r
+ l_imvertex[i].x = i_xcoord[i_mkvertex[i]]*2;\r
+ l_imvertex[i].y = i_ycoord[i_mkvertex[i]]*2;\r
+ }\r
+ return true;\r
+ }\r
+}\r
+\r
+\r
+\r
+/**\r
+ * ラベル同士の重なり（内包関係）を調べるクラスです。 \r
+ * ラベルリストに内包するラベルを蓄積し、それにターゲットのラベルが内包されているか を確認します。\r
+ */\r
+class OverlapChecker\r
+{\r
+ private NyARLabelingLabel[] _labels = new NyARLabelingLabel[32];\r
+\r
+ private int _length;\r
+\r
+ /**\r
+ * 最大i_max_label個のラベルを蓄積できるようにオブジェクトをリセットする\r
+ * \r
+ * @param i_max_label\r
+ */\r
+ public void reset(int i_max_label)\r
+ {\r
+ if (i_max_label > this._labels.length) {\r
+ this._labels = new NyARLabelingLabel[i_max_label];\r
+ }\r
+ this._length = 0;\r
+ }\r
+\r
+ /**\r
+ * チェック対象のラベルを追加する。\r
+ * \r
+ * @param i_label_ref\r
+ */\r
+ public void push(NyARLabelingLabel i_label_ref)\r
+ {\r
+ this._labels[this._length] = i_label_ref;\r
+ this._length++;\r
+ }\r
+\r
+ /**\r
+ * 現在リストにあるラベルと重なっているかを返す。\r
+ * \r
+ * @param i_label\r
+ * @return 何れかのラベルの内側にあるならばfalse,独立したラベルである可能性が高ければtrueです．\r
+ */\r
+ public boolean check(NyARLabelingLabel i_label)\r
+ {\r
+ // 重なり処理かな？\r
+ final NyARLabelingLabel[] label_pt = this._labels;\r
+ final int px1 = (int) i_label.pos_x;\r
+ final int py1 = (int) i_label.pos_y;\r
+ for (int i = this._length - 1; i >= 0; i--) {\r
+ final int px2 = (int) label_pt[i].pos_x;\r
+ final int py2 = (int) label_pt[i].pos_y;\r
+ final int d = (px1 - px2) * (px1 - px2) + (py1 - py2) * (py1 - py2);\r
+ if (d < label_pt[i].area / 4) {\r
+ // 対象外\r
+ return false;\r
+ }\r
+ }\r
+ // 対象\r
+ return true;\r
+ }\r
+}
\ No newline at end of file
private final NyARLabelingImage _limage;\r
\r
private final OverlapChecker _overlap_checker = new OverlapChecker();\r
- private final NyARCameraDistortionFactor _dist_factor_ref;\r
+ private final NyARCameraDistortionFactorMap _dist_factor_ref;\r
\r
/**\r
* 最大i_squre_max個のマーカーを検出するクラスを作成する。\r
* \r
* @param i_param\r
*/\r
- public NyARVertexDetector(NyARCameraDistortionFactor i_dist_factor_ref,NyARIntSize i_size) throws NyARException\r
+ public NyARVertexDetector(NyARCameraDistortionFactorMap i_dist_factor_ref,NyARIntSize i_size) throws NyARException\r
{\r
this._width = i_size.w;\r
this._height = i_size.h;\r
\r
private void getSquare(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square)\r
{\r
- final NyARCameraDistortionFactor dist_factor=this._dist_factor_ref; \r
+ final NyARCameraDistortionFactorMap dist_factor=this._dist_factor_ref; \r
final NyARDoublePoint2d[] vertex=o_square.sqvertex;\r
//歪み補正\r
for(int i=0;i<4;i++)\r
+++ /dev/null
-/* \r
- * PROJECT: NyARToolkit JOGL utilities.\r
- * --------------------------------------------------------------------------------\r
- * This work is based on the original ARToolKit developed by\r
- * Hirokazu Kato\r
- * Mark Billinghurst\r
- * HITLab, University of Washington, Seattle\r
- * http://www.hitl.washington.edu/artoolkit/\r
- *\r
- * The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
- *\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- * \r
- * For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
- * <airmail(at)ebony.plala.or.jp>\r
- * \r
- */\r
-package jp.nyatla.nyartoolkit.sandbox.x2;\r
-\r
-import jp.nyatla.nyartoolkit.NyARException;\r
-import jp.nyatla.nyartoolkit.core.NyARCode;\r
-import jp.nyatla.nyartoolkit.core.param.NyARParam;\r
-import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult;\r
-\r
-\r
-/**\r
- * NyARSingleDetectMarkerにOpenGL向け関数を追加したもの\r
- */\r
-public class GLNyARSingleDetectMarker_X2 extends NyARSingleDetectMarker_X2\r
-{\r
- private NyARTransMatResult trans_mat_result = new NyARTransMatResult();\r
-\r
- private double view_scale_factor = 0.025;// #define VIEW_SCALEFACTOR 0.025 // 1.0 ARToolKit unit becomes 0.025 of my OpenGL units.\r
-\r
- public GLNyARSingleDetectMarker_X2(NyARParam i_param, NyARCode i_code, double i_marker_width) throws NyARException\r
- {\r
- super(i_param, i_code, i_marker_width);\r
- }\r
-\r
- public void setScaleFactor(double i_new_value)\r
- {\r
- view_scale_factor = i_new_value;\r
- }\r
-\r
- // public static void arglCameraViewRH(const double para[3][4], GLdouble m_modelview[16], const double scale)\r
- public double[] getCameraViewRH() throws NyARException\r
- {\r
- double[] result = new double[16];\r
- getCameraViewRH(result);\r
- return result;\r
- }\r
-\r
- /**\r
- * \r
- * @param o_result\r
- * 結果値を格納する配列を指定してください。double[16]以上が必要です。\r
- * @throws NyARException\r
- */\r
- public void getCameraViewRH(double[] o_result) throws NyARException\r
- {\r
- // 座標を計算\r
- this.getTransmationMatrix(this.trans_mat_result);\r
- // 行列変換\r
- final NyARTransMatResult mat = this.trans_mat_result;\r
- o_result[0 + 0 * 4] = mat.m00; // R1C1\r
- o_result[0 + 1 * 4] = mat.m01; // R1C2\r
- o_result[0 + 2 * 4] = mat.m02;\r
- o_result[0 + 3 * 4] = mat.m03;\r
- o_result[1 + 0 * 4] = -mat.m10; // R2\r
- o_result[1 + 1 * 4] = -mat.m11;\r
- o_result[1 + 2 * 4] = -mat.m12;\r
- o_result[1 + 3 * 4] = -mat.m13;\r
- o_result[2 + 0 * 4] = -mat.m20; // R3\r
- o_result[2 + 1 * 4] = -mat.m21;\r
- o_result[2 + 2 * 4] = -mat.m22;\r
- o_result[2 + 3 * 4] = -mat.m23;\r
- o_result[3 + 0 * 4] = 0.0;\r
- o_result[3 + 1 * 4] = 0.0;\r
- o_result[3 + 2 * 4] = 0.0;\r
- o_result[3 + 3 * 4] = 1.0;\r
- if (view_scale_factor != 0.0) {\r
- o_result[12] *= view_scale_factor;\r
- o_result[13] *= view_scale_factor;\r
- o_result[14] *= view_scale_factor;\r
- }\r
- return;\r
- }\r
-}\r
private NyARGLUtil _glnya;\r
\r
//NyARToolkit関係\r
- private GLNyARSingleDetectMarker_X2 _nya;\r
+ private NyARSingleDetectMarker_X2 _nya;\r
\r
private NyARParam _ar_param;\r
\r
\r
public JavaSimpleLite_X2()\r
{\r
+ NyMath.initialize();\r
Frame frame = new Frame("Java simpleLite with NyARToolkit");\r
\r
// 3Dを描画するコンポーネント\r
NyARCode ar_code = new NyARCode(16, 16);\r
_ar_param.loadARParamFromFile(PARAM_FILE);\r
_ar_param.changeScreenSize(SCREEN_X, SCREEN_Y);\r
- _nya = new GLNyARSingleDetectMarker_X2(_ar_param, ar_code, 80.0);\r
+ _nya = new NyARSingleDetectMarker_X2(_ar_param, ar_code, 80.0);\r
_nya.setContinueMode(false);//ここをtrueにすると、transMatContinueモード（History計算）になります。\r
ar_code.loadARPattFromFile(CARCODE_FILE);\r
//NyARToolkit用の支援クラス\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+\r
+\r
+\r
+/**\r
+ * ARMat構造体に対応するクラス typedef struct { double *m; int row; int clm; }ARMat;\r
+ * \r
+ */\r
+public class NyARFixedFloat16Mat\r
+{\r
+ /**\r
+ * 配列サイズと行列サイズは必ずしも一致しないことに注意 返された配列のサイズを行列の大きさとして使わないこと！\r
+ * \r
+ */\r
+ protected long[][] m;\r
+ private int clm, row;\r
+\r
+ /**\r
+ * デフォルトコンストラクタは機能しません。\r
+ * \r
+ * @throws NyARException\r
+ */\r
+ protected NyARFixedFloat16Mat() throws NyARException\r
+ {\r
+ throw new NyARException();\r
+ }\r
+\r
+ public NyARFixedFloat16Mat(int i_row, int i_clm)\r
+ {\r
+ this.m = new long[i_row][i_clm];\r
+ clm = i_clm;\r
+ row = i_row;\r
+ return;\r
+ }\r
+ public int getClm()\r
+ {\r
+ return clm;\r
+ }\r
+ public int getRow()\r
+ {\r
+ return row;\r
+ }\r
+ /**\r
+ * 行列をゼロクリアする。\r
+ */\r
+ public void zeroClear()\r
+ {\r
+ int i, i2;\r
+ // For順変更OK\r
+ for (i = row - 1; i >= 0; i--) {\r
+ for (i2 = clm - 1; i2 >= 0; i2--) {\r
+ m[i][i2] = 0;\r
+ }\r
+ }\r
+ }\r
+ public long[][] getArray()\r
+ {\r
+ return this.m;\r
+ }\r
+ // public void getRowVec(int i_row,NyARVec o_vec)\r
+ // {\r
+ // o_vec.set(this.m[i_row],this.clm);\r
+ // }\r
+ /**\r
+ * aとbの積を自分自身に格納する。arMatrixMul()の代替品\r
+ * \r
+ * @param a\r
+ * @param b\r
+ * @throws NyARException\r
+ */\r
+ public void matrixMul(NyARFixedFloat16Mat a, NyARFixedFloat16Mat b) throws NyARException\r
+ {\r
+ if (a.clm != b.row || this.row != a.row || this.clm != b.clm) {\r
+ throw new NyARException();\r
+ }\r
+ long w;\r
+ int r, c, i;\r
+ long[][] am = a.m, bm = b.m, dm = this.m;\r
+ // For順変更禁止\r
+ for (r = 0; r < this.row; r++) {\r
+ for (c = 0; c < this.clm; c++) {\r
+ w = 0L;// dest.setARELEM0(r, c,0.0);\r
+ for (i = 0; i < a.clm; i++) {\r
+ w += (am[r][i] * bm[i][c])>>16;// ARELEM0(dest, r, c) +=ARELEM0(a, r, i) * ARELEM0(b,i, c);\r
+ }\r
+ dm[r][c] = w;\r
+ }\r
+ }\r
+ }\r
+}
\ No newline at end of file
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d;\r
+/**\r
+ * 歪み成分マップを使用するINyARCameraDistortionFactor\r
+ * 内部マップをint(1:15:16)フォーマットの固定小数点で保持する。\r
+ * 固定小数点で値を提供するインタフェイスを持ちます。\r
+ */\r
+final public class NyARFixedFloatCameraDistortionFactorMap\r
+{\r
+ private double[] _factor=new double[4];\r
+ private int _stride;\r
+ private int[] _mapx;\r
+ private int[] _mapy;\r
+ public NyARFixedFloatCameraDistortionFactorMap(NyARCameraDistortionFactor i_distfactor,NyARIntSize i_screen_size)\r
+ {\r
+ NyARDoublePoint2d opoint=new NyARDoublePoint2d();\r
+ this._mapx=new int[i_screen_size.w*i_screen_size.h];\r
+ this._mapy=new int[i_screen_size.w*i_screen_size.h];\r
+ this._stride=i_screen_size.w;\r
+ int ptr=i_screen_size.h*i_screen_size.w-1;\r
+ //歪みマップを構築\r
+ for(int i=i_screen_size.h-1;i>=0;i--)\r
+ {\r
+ for(int i2=i_screen_size.w-1;i2>=0;i2--)\r
+ {\r
+ i_distfactor.observ2Ideal(i2,i, opoint);\r
+ this._mapx[ptr]=(int)(opoint.x*65536);\r
+ this._mapy[ptr]=(int)(opoint.y*65536);\r
+ ptr--;\r
+ }\r
+ }\r
+ i_distfactor.getValue(this._factor);\r
+ return;\r
+ } \r
+ public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size)\r
+ {\r
+ double x, y;\r
+ final double d0 = this._factor[0];\r
+ final double d1 = this._factor[1];\r
+ final double d3 = this._factor[3];\r
+ final double d2_w = this._factor[2] / 100000000.0;\r
+ for (int i = 0; i < i_size; i++) {\r
+ x = (i_in[i].x - d0) * d3;\r
+ y = (i_in[i].y - d1) * d3;\r
+ if (x == 0.0 && y == 0.0) {\r
+ o_out[i].x = (long)(d0*NyMath.FIXEDFLOAT16_1);\r
+ o_out[i].y = (long)(d1*NyMath.FIXEDFLOAT16_1);\r
+ } else {\r
+ final double d = 1.0 - d2_w * (x * x + y * y);\r
+ o_out[i].x = (long)((x * d + d0)*NyMath.FIXEDFLOAT16_1);\r
+ o_out[i].y = (long)((y * d + d1)*NyMath.FIXEDFLOAT16_1);\r
+ }\r
+ }\r
+ return;\r
+ }\r
+ public void ideal2Observ(final NyARDoublePoint2d i_in, NyARFixedFloat16Point2d o_out)\r
+ {\r
+ final double f0=this._factor[0];\r
+ final double f1=this._factor[1];\r
+ final double x = (i_in.x - f0) * this._factor[3];\r
+ final double y = (i_in.y - f1) * this._factor[3];\r
+ if (x == 0.0 && y == 0.0) {\r
+ o_out.x = (long)(f0*NyMath.FIXEDFLOAT16_1);\r
+ o_out.y = (long)(f1*NyMath.FIXEDFLOAT16_1);\r
+ } else {\r
+ final double d = 1.0 - this._factor[2] / 100000000.0 * (x * x + y * y);\r
+ o_out.x = (long)((x * d + f0)*NyMath.FIXEDFLOAT16_1);\r
+ o_out.y = (long)((y * d + f1)*NyMath.FIXEDFLOAT16_1);\r
+ }\r
+ return;\r
+ } \r
+ /**\r
+ * 点集合のi_start～i_numまでの間から、最大i_sample_count個の頂点を取得して返します。\r
+ * i_sample_countは偶数である必要があります。\r
+ * @param i_x_coord\r
+ * @param i_y_coord\r
+ * @param i_start\r
+ * @param i_num\r
+ * @param o_x_coord\r
+ * @param o_y_coord\r
+ * @param i_sample_count\r
+ * @return\r
+ */\r
+ public int observ2IdealSampling(int[] i_x_coord, int[] i_y_coord,int i_start, int i_num, int[] o_x_coord,int[] o_y_coord,int i_sample_count)\r
+ {\r
+ assert(i_sample_count%2==0);\r
+ int idx;\r
+ if (i_num < i_sample_count)\r
+ {\r
+ for (int i = i_num - 1; i >= 0; i -= 2)\r
+ {\r
+ idx = i_x_coord[i_start + i] + i_y_coord[i_start + i] * this._stride;\r
+ o_x_coord[i] = this._mapx[idx];\r
+ o_y_coord[i] = this._mapy[idx];\r
+ }\r
+ return i_num;\r
+ }\r
+ else\r
+ {\r
+ //サンプリング個数分の点を、両端から半分づつ取ってくる。\r
+ int st = i_start;\r
+ int ed = i_start + i_num - 1;\r
+ for (int i = i_sample_count - 1; i >= 0; i -= 2)\r
+ {\r
+ idx = i_x_coord[st] + i_y_coord[st] * this._stride;\r
+ o_x_coord[i] = this._mapx[idx];\r
+ o_y_coord[i] = this._mapy[idx];\r
+ idx = i_x_coord[ed] + i_y_coord[ed] * this._stride;\r
+ o_x_coord[i - 1] = this._mapx[idx];\r
+ o_y_coord[i - 1] = this._mapy[idx];\r
+ ed--;\r
+ st++;\r
+ }\r
+ return i_sample_count;\r
+ }\r
+ }\r
+}
\ No newline at end of file
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.*;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d;\r
+import jp.nyatla.nyartoolkit.core.*;\r
+\r
+class NyARCustomMatrix extends NyARMat\r
+{\r
+ public NyARCustomMatrix(int i_row, int i_clm)\r
+ {\r
+ super(i_row,i_clm);\r
+ return;\r
+ }\r
+ public void copyFrom(NyARFixedFloat16Mat i_mat) throws NyARException\r
+ {\r
+ long[][] ptr;\r
+ int r,c;\r
+ // For順変更禁止\r
+ for (r = 0; r < this.row; r++){\r
+ ptr=i_mat.getArray();\r
+ for (c = 0; c < this.clm; c++){\r
+ this.m[c][r]=(double)ptr[c][r]/0x10000;\r
+ }\r
+ }\r
+ return;\r
+ }\r
+}\r
+/**\r
+ * 平行移動量を計算するクラス\r
+ * \r
+ * NyARPerspectiveProjectionMatrixに直接アクセスしてる場所があるけど、\r
+ * この辺の計算はNyARPerspectiveProjectionMatrixクラスの関数にして押し込む予定。\r
+ *\r
+ */\r
+public class NyARFixedFloatFitVecCalculator\r
+{\r
+ private final NyARFixedFloat16Mat _mat_b = new NyARFixedFloat16Mat(3,8);//3,NUMBER_OF_VERTEX*2\r
+ private final NyARFixedFloat16Mat _mat_a = new NyARFixedFloat16Mat(8,3);/*NUMBER_OF_VERTEX,3*/\r
+ private final NyARFixedFloat16Mat _mat_d = new NyARFixedFloat16Mat(3,3);\r
+ private final NyARCustomMatrix _mat_d2=new NyARCustomMatrix(3,3);\r
+ private final NyARPerspectiveProjectionMatrix _projection_mat;\r
+ private final NyARFixedFloatCameraDistortionFactorMap _distortionfactor;\r
+\r
+\r
+// private NyARDoublePoint2d[] _vertex_2d_ref;\r
+ public NyARFixedFloatFitVecCalculator(final NyARPerspectiveProjectionMatrix i_projection_mat_ref,final NyARFixedFloatCameraDistortionFactorMap i_distortion_ref)\r
+ {\r
+ // 変換マトリクスdとbの準備(arGetTransMatSubの一部)\r
+ final long[][] a_array = this._mat_a.getArray();\r
+ final long[][] b_array = this._mat_b.getArray();\r
+\r
+ //変換用行列のcpara固定値の部分を先に初期化してしまう。\r
+ for (int i = 0; i < 4; i++) {\r
+ final int x2 = i * 2;\r
+ a_array[x2][0] = b_array[0][x2] =(long)(i_projection_mat_ref.m00*NyMath.FIXEDFLOAT16_1);// mat_a->m[j*6+0]=mat_b->m[num*0+j*2] =cpara[0][0];\r
+ a_array[x2][1] = b_array[1][x2] =(long)(i_projection_mat_ref.m01*NyMath.FIXEDFLOAT16_1);// mat_a->m[j*6+1]=mat_b->m[num*2+j*2]=cpara[0][1];\r
+ a_array[x2 + 1][0] = b_array[0][x2 + 1] =0;// mat_a->m[j*6+3] =mat_b->m[num*0+j*2+1]= 0.0;\r
+ a_array[x2 + 1][1] = b_array[1][x2 + 1] =(long)(i_projection_mat_ref.m11*NyMath.FIXEDFLOAT16_1);// mat_a->m[j*6+4] =mat_b->m[num*2+j*2+1]= cpara[1][1];\r
+ }\r
+ this._projection_mat=i_projection_mat_ref;\r
+ this._distortionfactor=i_distortion_ref;\r
+ return;\r
+ }\r
+ private final NyARFixedFloat16Point2d[] _fitsquare_vertex=NyARFixedFloat16Point2d.createArray(4);;\r
+ private NyARFixedFloatTransOffset _offset_square;\r
+ public void setOffsetSquare(NyARFixedFloatTransOffset i_offset)\r
+ {\r
+ this._offset_square=i_offset;\r
+ return;\r
+ }\r
+ public NyARFixedFloat16Point2d[] getFitSquare()\r
+ {\r
+ return this._fitsquare_vertex;\r
+ }\r
+ public NyARFixedFloatTransOffset getOffsetVertex()\r
+ {\r
+ return this._offset_square;\r
+ }\r
+\r
+ /**\r
+ * 適合させる矩形座標を指定します。\r
+ * @param i_square_vertex\r
+ * @throws NyARException\r
+ */\r
+ public void setFittedSquare(NyARDoublePoint2d[] i_square_vertex) throws NyARException\r
+ {\r
+ final NyARFixedFloat16Point2d[] vertex=_fitsquare_vertex;\r
+ for(int i=0;i<4;i++){\r
+ this._distortionfactor.ideal2Observ(i_square_vertex[i], vertex[i]);\r
+ } \r
+ \r
+ final long cpara02=(long)(this._projection_mat.m02*NyMath.FIXEDFLOAT16_1);\r
+ final long cpara12=(long)(this._projection_mat.m12*NyMath.FIXEDFLOAT16_1); \r
+ final NyARFixedFloat16Mat mat_d=_mat_d;\r
+ final NyARFixedFloat16Mat mat_a=this._mat_a;\r
+ final NyARFixedFloat16Mat mat_b=this._mat_b;\r
+ final long[][] a_array = mat_a.getArray();\r
+ final long[][] b_array = mat_b.getArray();\r
+ for (int i = 0; i < 4; i++) {\r
+ final int x2 = i * 2; \r
+ a_array[x2][2] = b_array[2][x2] = (long)((cpara02 - vertex[i].x));// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];\r
+ a_array[x2 + 1][2] = b_array[2][x2 + 1] = (long)((cpara12 - vertex[i].y));// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];\r
+ }\r
+ // mat_d\r
+ mat_d.matrixMul(mat_b, mat_a);\r
+ this._mat_d2.copyFrom(mat_d);\r
+ this._mat_d2.matrixSelfInv(); \r
+ return;\r
+ }\r
+ private final NyARFixedFloat16Mat _mat_e = new NyARFixedFloat16Mat(3, 1);\r
+ private final NyARFixedFloat16Mat __calculateTransferVec_mat_c = new NyARFixedFloat16Mat(8, 1);//NUMBER_OF_VERTEX * 2, 1\r
+ private final NyARFixedFloat16Point3d[] __calculateTransfer_point3d=NyARFixedFloat16Point3d.createArray(4); \r
+ \r
+ /**\r
+ * 現在のオフセット矩形、適合先矩形と、回転行列から、平行移動量を計算します。\r
+ * @param i_rotation\r
+ * @param o_transfer\r
+ * @throws NyARException\r
+ */\r
+ final public void calculateTransfer(NyARFixedFloatRotMatrix i_rotation,NyARFixedFloat16Point3d o_transfer) throws NyARException\r
+ {\r
+ assert(this._offset_square!=null);\r
+ final long cpara00=(long)(this._projection_mat.m00*NyMath.FIXEDFLOAT16_1);\r
+ final long cpara01=(long)(this._projection_mat.m01*NyMath.FIXEDFLOAT16_1);\r
+ final long cpara02=(long)(this._projection_mat.m02*NyMath.FIXEDFLOAT16_1);\r
+ final long cpara11=(long)(this._projection_mat.m11*NyMath.FIXEDFLOAT16_1);\r
+ final long cpara12=(long)(this._projection_mat.m12*NyMath.FIXEDFLOAT16_1);\r
+ \r
+ final NyARFixedFloat16Point3d[] point3d=this.__calculateTransfer_point3d;\r
+ final NyARFixedFloat16Point3d[] vertex3d=this._offset_square.vertex; \r
+ final NyARFixedFloat16Point2d[] vertex2d=this._fitsquare_vertex;\r
+ final NyARFixedFloat16Mat mat_c = this.__calculateTransferVec_mat_c;// 次処理で値をもらうので、初期化の必要は無い。\r
+ \r
+ final long[][] c_array = mat_c.getArray();\r
+ \r
+ \r
+ //（3D座標？）を一括請求\r
+ i_rotation.getPoint3dBatch(vertex3d,point3d,4);\r
+ for (int i = 0; i < 4; i++) {\r
+ final int x2 = i+i;\r
+ final NyARFixedFloat16Point3d point3d_ptr=point3d[i];\r
+ //透視変換？\r
+ c_array[x2][0] =(long)((point3d_ptr.z * vertex2d[i].x - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z)>>16);// mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz;\r
+ c_array[x2 + 1][0] =(long)((point3d_ptr.z * vertex2d[i].y - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z)>>16);// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz;\r
+ }\r
+ this._mat_e.matrixMul(this._mat_b, mat_c);\r
+ final double[][] d2=this._mat_d2.getArray();\r
+ final long[][] e2=this._mat_e.getArray();\r
+ \r
+ \r
+ //this._mat_f.matrixMul(this._mat_d, this._mat_e);\r
+\r
+ // double[] trans=wk_arGetTransMatSub_trans;//double trans[3];\r
+ o_transfer.x=(long)(d2[0][0]*e2[0][0]+d2[0][1]*e2[1][0]+d2[0][2]*e2[2][0]);\r
+ o_transfer.y=(long)(d2[1][0]*e2[0][0]+d2[1][1]*e2[1][0]+d2[1][2]*e2[2][0]);\r
+ o_transfer.z=(long)(d2[2][0]*e2[0][0]+d2[2][1]*e2[1][0]+d2[2][2]*e2[2][0]);\r
+ return;\r
+ }\r
+ \r
+ \r
+ \r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core.pca2d.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARI64Point2d;\r
+import jp.nyatla.nyartoolkit.core2.types.matrix.NyARI64Matrix22;\r
+/**\r
+ * 64bit(小数部16bit)の固定小数点を利用したPCA関数\r
+ *\r
+ */\r
+public class NyARFixedFloatPca2d implements INyARPca2d\r
+{\r
+// private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
+\r
+ private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\r
+ /**\r
+ * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
+ * \r
+ * @param a\r
+ * @param dv\r
+ * @throws NyARException\r
+ */\r
+ private static void PCA_QRM(NyARI64Matrix22 o_matrix, NyARI64Point2d dv) throws NyARException\r
+ {\r
+ long abs_x;\r
+ long w, t, s, x, y, c;\r
+ long ev1;\r
+ long dv_x,dv_y;\r
+ long mat00,mat01,mat10,mat11;\r
+ // <this.vecTridiagonalize2d(i_mat, dv, ev);>\r
+ dv_x =o_matrix.m00;// 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)];\r
+ ev1 =o_matrix.m01;// 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)];\r
+ dv_y =o_matrix.m11;// 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)];\r
+ // 単位行列にする。\r
+ mat00 = mat11 = (1<<16);\r
+ mat01 = mat10 = 0;\r
+ // </this.vecTridiagonalize2d(i_mat, dv, ev);>\r
+\r
+ // int j = 1;\r
+ // // while(j>0 && fabs(ev->v[j])>EPS*(fabs(dv->v[j-1])+fabs(dv->v[j])))\r
+ // while (j > 0 && Math.abs(ev1) > PCA_EPS * (Math.abs(dv.x) + Math.abs(dv.y))) {\r
+ // j--;\r
+ // }\r
+ // if (j == 0) {\r
+ int iter = 0;\r
+ do {\r
+ iter++;\r
+ if (iter > PCA_MAX_ITER) {\r
+ break;\r
+ }\r
+ w = (dv_x - dv_y) / 2L;//S16\r
+ t = (ev1 * ev1)>>16;// t = ev->v[h] * ev->v[h];\r
+ s = NyMath.sqrtFixdFloat16(((w * w)>>16) + t);\r
+ if (w < 0) {\r
+ s = -s;\r
+ }\r
+ x = dv_x - dv_y + (t<<16) / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);\r
+ y = ev1;// y = ev->v[j+1];\r
+\r
+ abs_x=(x>0?x:-x);\r
+ if (abs_x >= (y>0?y:-y)) { //if (Math.abs(x) >= Math.abs(y)) {\r
+ if ((abs_x>>16) > 0) {\r
+ t = -(y<<16) / x;\r
+ c = (1L<<32) / NyMath.sqrtFixdFloat16(((t * t)>>16) + (1L<<16));\r
+ s = (t * c)>>16;\r
+ } else {\r
+ c = (1L<<16);\r
+ s = 0;\r
+ }\r
+ } else {\r
+ t = -(x<<16) / y;\r
+ s = (1L<<32) / NyMath.sqrtFixdFloat16(((t * t)>>16) + (1L<<16));\r
+ c = (t * s)>>16;\r
+ }\r
+ w = dv_x - dv_y;// w = dv->v[k] -dv->v[k+1];\r
+ t = (((w * s + 2L * c * ev1)>>16) * s)>>16;// t = (w * s +2 * c *ev->v[k+1]) *s;\r
+ dv_x -= t;// dv->v[k] -= t;\r
+ dv_y += t;// dv->v[k+1] += t;\r
+ ev1 += (s * ((c * w - 2L * s * ev1)>>16)>>16);// ev->v[k+1]+= s * (c* w- 2* s *ev->v[k+1]);\r
+\r
+ x = mat00;// x = a->m[k*dim+i];\r
+ y = mat10;// y = a->m[(k+1)*dim+i];\r
+ mat00 = (c * x - s * y)>>16;// a->m[k*dim+i] = c * x - s* y;\r
+ mat10 = (s * x + c * y)>>16;// a->m[(k+1)*dim+i] = s* x + c * y;\r
+ \r
+ x = mat01;// x = a->m[k*dim+i];\r
+ y = mat11;// y = a->m[(k+1)*dim+i];\r
+ mat01 = (c * x - s * y)>>16;// a->m[k*dim+i] = c * x - s* y;\r
+ mat11 = (s * x + c * y)>>16;// a->m[(k+1)*dim+i] = s* x + c * y;\r
+ } while (((ev1>0?ev1:-ev1)>>16) > ((((dv_x>0?dv_x:-dv_x) + (dv_y>0?dv_y:-dv_y))>>16)/1000000L));\r
+ // }\r
+\r
+ t = dv_x;// t = dv->v[h];\r
+ if (dv_y > t) {// if( dv->v[i] > t ) {\r
+ t = dv_y;// t = dv->v[h];\r
+ dv_y = dv_x;// dv->v[h] = dv->v[k];\r
+ dv_x = t;// dv->v[k] = t;\r
+ // 行の入れ替え\r
+ o_matrix.m00 = mat10;\r
+ o_matrix.m01 = mat11;\r
+ o_matrix.m10 = mat00; \r
+ o_matrix.m11 = mat01;\r
+ \r
+ } else {\r
+ // 行の入れ替えはなし\r
+ o_matrix.m00 = mat00;\r
+ o_matrix.m01 = mat01;\r
+ o_matrix.m10 = mat10; \r
+ o_matrix.m11 = mat11;\r
+ }\r
+ dv.x=dv_x;\r
+ dv.y=dv_y;\r
+ return; \r
+ }\r
+\r
+ /**\r
+ * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
+ * \r
+ * @param output\r
+ * @param o_ev\r
+ * @throws NyARException\r
+ */\r
+ /**\r
+ * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
+ * \r
+ * @param output\r
+ * @param o_ev\r
+ * @throws NyARException\r
+ */\r
+ private void PCA_PCA(int[] i_x,int[] i_y,int i_number_of_data,NyARI64Matrix22 o_matrix, NyARI64Point2d o_ev,NyARI64Point2d o_mean) throws NyARException\r
+ {\r
+ // double[] mean_array=mean.getArray();\r
+ // mean.zeroClear();\r
+\r
+ //PCA_EXの処理\r
+ long sx = 0;\r
+ long sy = 0;\r
+ for (int i = 0; i < i_number_of_data; i++) {\r
+ sx += i_x[i];//S16\r
+ sy += i_y[i];//S16\r
+ }\r
+ sx = sx / i_number_of_data;//S16\r
+ sy = sy / i_number_of_data;//S16\r
+ \r
+ //PCA_CENTERとPCA_xt_by_xを一緒に処理\r
+ final long srow = NyMath.sqrtFixdFloat16((long)i_number_of_data<<16);//S16\r
+ long w00, w11, w10;\r
+ w00 = w11 = w10 = 0L;// *out = 0.0;\r
+ for (int i = 0; i < i_number_of_data; i++) {\r
+ final long x = ((i_x[i] - sx)<<16) / srow;//S6\r
+ final long y = ((i_y[i] - sy)<<16) / srow;//S6\r
+ w00 += (x * x)>>16;//S16\r
+ w10 += (x * y)>>16;//S16\r
+ w11 += (y * y)>>16;//S16\r
+ }\r
+ o_matrix.m00=w00;\r
+ o_matrix.m01=o_matrix.m10=w10;\r
+ o_matrix.m11=w11;\r
+ \r
+ //PCA_PCAの処理\r
+ PCA_QRM(o_matrix, o_ev);\r
+ // m2 = o_output.m;// m2 = output->m;\r
+ if (o_ev.x < 0) {// if( ev->v[i] < VZERO ){\r
+ o_ev.x = 0;// ev->v[i] = 0.0;\r
+ o_matrix.m00 = 0;// *(m2++) = 0.0;\r
+ o_matrix.m01 = 0;// *(m2++) = 0.0;\r
+ }\r
+\r
+ if (o_ev.y < 0) {// if( ev->v[i] < VZERO ){\r
+ o_ev.y = 0;// ev->v[i] = 0.0;\r
+ o_matrix.m10 = 0;// *(m2++) = 0.0;\r
+ o_matrix.m11 = 0;// *(m2++) = 0.0;\r
+ }\r
+ o_mean.x=sx;\r
+ o_mean.y=sy;\r
+ // }\r
+ return;\r
+ }\r
+ private int[] __pca_tmpx=null;\r
+ private int[] __pca_tmpy=null;\r
+ private NyARI64Matrix22 __pca_evec=null;\r
+ private NyARI64Point2d __pca_ev=null;\r
+ private NyARI64Point2d __pca_mean=null;\r
+ public void pca(double[] i_x,double[] i_y,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
+ {\r
+ //変換用のワーク変数作成\r
+ if(__pca_tmpx==null)\r
+ {\r
+ this.__pca_tmpx=new int[i_number_of_point];\r
+ this.__pca_tmpy=new int[i_number_of_point];\r
+ this.__pca_evec=new NyARI64Matrix22 ();\r
+ this.__pca_ev=new NyARI64Point2d ();\r
+ this.__pca_mean=new NyARI64Point2d ();\r
+ }else if(i_number_of_point>this.__pca_tmpx.length)\r
+ {\r
+ this.__pca_tmpx=new int[i_number_of_point];\r
+ this.__pca_tmpy=new int[i_number_of_point];\r
+ }\r
+ //値のセット\r
+ final int[] x_ptr=this.__pca_tmpx;\r
+ final int[] y_ptr=this.__pca_tmpy;\r
+ for(int i=0;i<i_number_of_point;i++)\r
+ {\r
+ x_ptr[i]=(int)(i_x[i]*65536L);\r
+ y_ptr[i]=(int)(i_y[i]*65536L);\r
+ }\r
+ //計算\r
+ pcaF16(x_ptr,y_ptr,i_number_of_point,this.__pca_evec,this.__pca_ev,this.__pca_mean);\r
+ //結果値を変換\r
+ o_evec.m00=(double)this.__pca_evec.m00/65536.0;\r
+ o_evec.m01=(double)this.__pca_evec.m01/65536.0;\r
+ o_evec.m10=(double)this.__pca_evec.m10/65536.0;\r
+ o_evec.m11=(double)this.__pca_evec.m11/65536.0;\r
+ o_ev.x=this.__pca_ev.x/65536.0;\r
+ o_ev.y=this.__pca_ev.y/65536.0;\r
+ o_mean.x=this.__pca_mean.x/65536.0;\r
+ o_mean.y=this.__pca_mean.y/65536.0;\r
+ return; \r
+ }\r
+ /**\r
+ * 値は全て小数点部16bitの固定小数点です。\r
+ * @param i_x\r
+ * @param i_y\r
+ * @param i_number_of_point\r
+ * @param o_evec\r
+ * @param o_ev\r
+ * @param o_mean\r
+ * @throws NyARException\r
+ */\r
+ public void pcaF16(int[] i_x,int[] i_y,int i_number_of_point,NyARI64Matrix22 o_evec, NyARI64Point2d o_ev,NyARI64Point2d o_mean) throws NyARException\r
+ {\r
+ //計算\r
+ PCA_PCA(i_x,i_y,i_number_of_point,o_evec,o_ev,o_mean);\r
+ final long sum = o_ev.x + o_ev.y;\r
+ // For順変更禁止\r
+ o_ev.x = (o_ev.x<<16)/sum;// ev->v[i] /= sum;\r
+ o_ev.y = (o_ev.y<<16)/sum;// ev->v[i] /= sum;\r
+ return; \r
+ } \r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d;\r
+import jp.nyatla.nyartoolkit.core2.types.matrix.NyARFixedFloat24Matrix33;\r
+\r
+\r
+\r
+/**\r
+ * 回転行列計算用の、3x3行列\r
+ * \r
+ */\r
+public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33\r
+{\r
+ /**\r
+ * インスタンスを準備します。\r
+ * \r
+ * @param i_param\r
+ */\r
+ public NyARFixedFloatRotMatrix(NyARPerspectiveProjectionMatrix i_matrix) throws NyARException\r
+ {\r
+ this.__initRot_vec1 = new NyARFixedFloatRotVector(i_matrix);\r
+ this.__initRot_vec2 = new NyARFixedFloatRotVector(i_matrix);\r
+ return;\r
+ }\r
+\r
+ final private NyARFixedFloatRotVector __initRot_vec1;\r
+\r
+ final private NyARFixedFloatRotVector __initRot_vec2;\r
+\r
+ public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
+ {\r
+ this.m00 =(long)(i_prev_result.m00*0x1000000);\r
+ this.m01 =(long)(i_prev_result.m01*0x1000000);\r
+ this.m02 =(long)(i_prev_result.m02*0x1000000);\r
+\r
+ this.m10 =(long)(i_prev_result.m10*0x1000000);\r
+ this.m11 =(long)(i_prev_result.m11*0x1000000);\r
+ this.m12 =(long)(i_prev_result.m12*0x1000000);\r
+\r
+ this.m20 =(long)(i_prev_result.m20*0x1000000);\r
+ this.m21 =(long)(i_prev_result.m21*0x1000000);\r
+ this.m22 =(long)(i_prev_result.m22*0x1000000);\r
+ return;\r
+ }\r
+\r
+ public final void initRotBySquare(final NyARLinear[] i_linear, final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
+ {\r
+ final NyARFixedFloatRotVector vec1 = this.__initRot_vec1;\r
+ final NyARFixedFloatRotVector vec2 = this.__initRot_vec2;\r
+\r
+ // 向かい合った辺から、２本のベクトルを計算\r
+\r
+ // 軸１\r
+ vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);\r
+ vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);\r
+\r
+ // 軸２\r
+ vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);\r
+ vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);\r
+\r
+ // 回転の最適化？\r
+ NyARFixedFloatRotVector.checkRotation(vec1, vec2);\r
+\r
+ this.m00 = (long)(vec1.v1*NyMath.FIXEDFLOAT24_1);\r
+ this.m10 = (long)(vec1.v2*NyMath.FIXEDFLOAT24_1);\r
+ this.m20 = (long)(vec1.v3*NyMath.FIXEDFLOAT24_1);\r
+ this.m01 = (long)(vec2.v1*NyMath.FIXEDFLOAT24_1);\r
+ this.m11 = (long)(vec2.v2*NyMath.FIXEDFLOAT24_1);\r
+ this.m21 = (long)(vec2.v3*NyMath.FIXEDFLOAT24_1);\r
+\r
+ // 最後の軸を計算\r
+ final long w02 = (long)((vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2)*NyMath.FIXEDFLOAT24_1);//S24\r
+ final long w12 = (long)((vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3)*NyMath.FIXEDFLOAT24_1);//S24\r
+ final long w22 = (long)((vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1)*NyMath.FIXEDFLOAT24_1);//S24\r
+ final long w = NyMath.sqrtFixdFloat((w02 * w02 + w12 * w12 + w22 * w22)>>24,24);//S24\r
+ this.m02 = (w02<<24) / w;\r
+ this.m12 = (w12<<24) / w;\r
+ this.m22 = (w22<<24) / w;\r
+ return;\r
+ }\r
+\r
+ /**\r
+ * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) Optimize:2008.04.20:STEP[481→433] 3x3変換行列から、回転角を復元して返します。\r
+ * \r
+ * @param o_angle\r
+ * @return\r
+ */\r
+ public final void getAngle(final NyARFixedFloat16Point3d o_angle)\r
+ {\r
+ int a, b, c;\r
+ long sina, cosa, sinb, cosb, sinc, cosc;\r
+\r
+ if (this.m22 > NyMath.FIXEDFLOAT24_1) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
+ this.m22 = NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = 1.0;\r
+ } else if (this.m22 < -NyMath.FIXEDFLOAT24_1) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
+ this.m22 = -NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = -1.0;\r
+ }\r
+ cosb = this.m22;// <Optimize/>cosb = rot[2][2];\r
+ b=(int)(Math.acos((double)cosb/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1);\r
+ sinb = (long)NyMath.sinFixedFloat24(b);\r
+ final long rot02 = this.m02;\r
+ final long rot12 = this.m12;\r
+ if (b!=0) {\r
+ cosa = (rot02<<24) / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
+ sina = (rot12<<24) / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
+ if (cosa > NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("cos(alph) = %f\n", cosa); */\r
+ cosa = NyMath.FIXEDFLOAT24_1;\r
+ sina = 0;\r
+ }\r
+ if (cosa < -NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("cos(alph) = %f\n", cosa); */\r
+ cosa = -NyMath.FIXEDFLOAT24_1;\r
+ sina = 0;\r
+ }\r
+ if (sina > NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("sin(alph) = %f\n", sina); */\r
+ sina = NyMath.FIXEDFLOAT24_1;\r
+ cosa = 0;\r
+ }\r
+ if (sina < -NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("sin(alph) = %f\n", sina); */\r
+ sina = -NyMath.FIXEDFLOAT24_1;\r
+ cosa = 0;\r
+ }\r
+ a = (int)(Math.acos((double)cosa/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1);\r
+ if (sina < 0) {\r
+ a = -a;\r
+ }\r
+ // <Optimize>\r
+ // 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]);\r
+ // 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]);\r
+ final long tmp = (rot02 * rot02 + rot12 * rot12)>>24;\r
+ sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;\r
+ cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;\r
+ // </Optimize>\r
+\r
+ if (cosc > NyMath.FIXEDFLOAT24_1){\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = NyMath.FIXEDFLOAT24_1;\r
+ sinc = 0;\r
+ }\r
+ if (cosc < -NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = -NyMath.FIXEDFLOAT24_1;\r
+ sinc = 0;\r
+ }\r
+ if (sinc > NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = NyMath.FIXEDFLOAT24_1;\r
+ cosc = 0;\r
+ }\r
+ if (sinc < -NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = -NyMath.FIXEDFLOAT24_1;\r
+ cosc = 0;\r
+ }\r
+ c = (int)(Math.acos((double)cosc/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1);\r
+ if (sinc < 0) {\r
+ c = -c;\r
+ }\r
+ } else {\r
+ a = b = 0;\r
+ cosa = cosb = NyMath.FIXEDFLOAT24_1;\r
+ sina = sinb = 0;\r
+ cosc = this.m00;// cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
+ sinc = this.m01;// sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
+ if (cosc > NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = NyMath.FIXEDFLOAT24_1;\r
+ sinc = 0;\r
+ }\r
+ if (cosc < -NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = -NyMath.FIXEDFLOAT24_1;\r
+ sinc = 0;\r
+ }\r
+ if (sinc > NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = NyMath.FIXEDFLOAT24_1;\r
+ cosc = 0;\r
+ }\r
+ if (sinc < -NyMath.FIXEDFLOAT24_1) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = -NyMath.FIXEDFLOAT24_1;\r
+ cosc = 0;\r
+ }\r
+ c = (int)(Math.acos((double)cosc/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1);\r
+ if (sinc < 0) {\r
+ c = -c;\r
+ }\r
+ }\r
+ o_angle.x = (long)a;// wa.value=a;//*wa = a;\r
+ o_angle.y = (long)b;// wb.value=b;//*wb = b;\r
+ o_angle.z = (long)c;// wc.value=c;//*wc = c;\r
+ return;\r
+ }\r
+ public final void setAngle(int i_x, int i_y, int i_z)\r
+ {\r
+ /*\r
+ * |cos(a) -sin(a) 0| |cos(b) 0 sin(b)| |cos(a-c) sin(a-c) 0| rot = |sin(a) cos(a) 0| |0 1 0 | |-sin(a-c) cos(a-c) 0| |0 0 1| |-sin(b) 0 cos(b)| |0 0 1|\r
+ */\r
+\r
+ long Sa, Sb, Ca, Cb, Sac, Cac, CaCb, SaCb;\r
+ Sa = NyMath.sinFixedFloat24(i_x);\r
+ Ca = NyMath.cosFixedFloat24(i_x);\r
+ Sb = NyMath.sinFixedFloat24(i_y);\r
+ Cb = NyMath.cosFixedFloat24(i_y);\r
+ Sac = NyMath.sinFixedFloat24(i_x - i_z);\r
+ Cac = NyMath.cosFixedFloat24(i_x - i_z);\r
+ CaCb =(Ca * Cb)>>24;\r
+ SaCb =(Sa * Cb)>>24;\r
+\r
+ this.m00 =(CaCb * Cac + Sa * Sac)>>24;\r
+ this.m01 =(CaCb * Sac - Sa * Cac)>>24;\r
+ this.m02 =(Ca * Sb)>>24;\r
+ this.m10 =(SaCb * Cac - Ca * Sac)>>24;\r
+ this.m11 =(SaCb * Sac + Ca * Cac)>>24;\r
+ this.m12 =(Sa * Sb)>>24;\r
+ this.m20 =(-Sb * Cac)>>24;\r
+ this.m21 =(-Sb * Sac)>>24;\r
+ this.m22 =Cb;\r
+ return;\r
+ }\r
+ /**\r
+ * i_in_pointを変換行列で座標変換する。\r
+ * \r
+ * @param i_in_point\r
+ * @param i_out_point\r
+ */\r
+ public final void getPoint3d(final NyARFixedFloat16Point3d i_in_point, final NyARFixedFloat16Point3d i_out_point)\r
+ {\r
+ i_out_point.x = (this.m00 * i_in_point.x + this.m01 * i_in_point.y + this.m02 * i_in_point.z)>>24;\r
+ i_out_point.y = (this.m10 * i_in_point.x + this.m11 * i_in_point.y + this.m12 * i_in_point.z)>>24;\r
+ i_out_point.z = (this.m20 * i_in_point.x + this.m21 * i_in_point.y + this.m22 * i_in_point.z)>>24;\r
+ return;\r
+ }\r
+\r
+ /**\r
+ * 複数の頂点を一括して変換する\r
+ * \r
+ * @param i_in_point\r
+ * @param i_out_point\r
+ * @param i_number_of_vertex\r
+ */\r
+ public final void getPoint3dBatch(final NyARFixedFloat16Point3d[] i_in_point, NyARFixedFloat16Point3d[] i_out_point, int i_number_of_vertex)\r
+ {\r
+ for (int i = i_number_of_vertex - 1; i >= 0; i--) {\r
+ final NyARFixedFloat16Point3d out_ptr = i_out_point[i];\r
+ final NyARFixedFloat16Point3d in_ptr = i_in_point[i];\r
+ out_ptr.x =(this.m00 * in_ptr.x + this.m01 * in_ptr.y + this.m02 * in_ptr.z)>>24;\r
+ out_ptr.y =(this.m10 * in_ptr.x + this.m11 * in_ptr.y + this.m12 * in_ptr.z)>>24;\r
+ out_ptr.z =(this.m20 * in_ptr.x + this.m21 * in_ptr.y + this.m22 * in_ptr.z)>>24;\r
+ }\r
+ return;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d;\r
+\r
+/**\r
+ * 基本姿勢と実画像を一致するように、角度を微調整→平行移動量を再計算\r
+ * を繰り返して、変換行列を最適化する。\r
+ *\r
+ */\r
+public class NyARFixedFloatRotTransOptimize_O2\r
+{\r
+ private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
+ private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
+ public NyARFixedFloatRotTransOptimize_O2(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
+ {\r
+ this._projection_mat_ref=i_projection_mat_ref;\r
+ return;\r
+ }\r
+ \r
+ final public double optimize(NyARFixedFloatRotMatrix io_rotmat,NyARFixedFloat16Point3d io_transvec,NyARFixedFloatFitVecCalculator i_calculator) throws NyARException\r
+ {\r
+ final NyARFixedFloat16Point2d[] fit_vertex=i_calculator.getFitSquare();\r
+ final NyARFixedFloat16Point3d[] offset_square=i_calculator.getOffsetVertex().vertex;\r
+ \r
+ double err = -1;\r
+ err = modifyMatrix(io_rotmat,io_transvec,offset_square,fit_vertex);\r
+ /*ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。*/\r
+ for (int i = 0; ; i++)\r
+ {\r
+ // <arGetTransMat3>\r
+ err = modifyMatrix(io_rotmat, io_transvec, offset_square, fit_vertex);\r
+ i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
+ err = modifyMatrix(io_rotmat, io_transvec, offset_square, fit_vertex);\r
+ // //</arGetTransMat3>\r
+ if (err < 1.0 || i == AR_GET_TRANS_MAT_MAX_LOOP_COUNT - 1)\r
+ {\r
+ break;\r
+ }\r
+ i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
+ }\r
+ return err;\r
+ }\r
+ \r
+ private final long[][] __modifyMatrix_double1D = new long[8][3];\r
+ private final NyARFixedFloat16Point3d __modifyMatrix_angle = new NyARFixedFloat16Point3d();\r
+ private final long INITIAL_FACTOR=(long)(0x10000*5.0 * Math.PI / 180.0);\r
+ /**\r
+ * arGetRot計算を階層化したModifyMatrix 896\r
+ * \r
+ * @param nyrot\r
+ * @param trans\r
+ * @param i_vertex3d\r
+ * [m][3]\r
+ * @param i_vertex2d\r
+ * [n][2]\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ private double modifyMatrix(NyARFixedFloatRotMatrix io_rot,NyARFixedFloat16Point3d trans, NyARFixedFloat16Point3d[] i_vertex3d, NyARFixedFloat16Point2d[] i_vertex2d) throws NyARException\r
+ {\r
+ long a2, b2, c2;\r
+ long h, x, y;\r
+ long err, minerr = 0;\r
+ int t1, t2, t3;\r
+ int best_idx=0;\r
+\r
+ long factor = INITIAL_FACTOR;\r
+ long rot0, rot1, rot2;\r
+ long combo00, combo01, combo02, combo03, combo10, combo11, combo12, combo13, combo20, combo21, combo22, combo23;\r
+ long combo02_2, combo02_5, combo02_8, combo02_11;\r
+ long combo22_2, combo22_5, combo22_8, combo22_11;\r
+ long combo12_2, combo12_5, combo12_8, combo12_11;\r
+ // vertex展開\r
+ final long VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32;\r
+ VX00 = i_vertex3d[0].x;\r
+ VX01 = i_vertex3d[0].y;\r
+ VX02 = i_vertex3d[0].z;\r
+ VX10 = i_vertex3d[1].x;\r
+ VX11 = i_vertex3d[1].y;\r
+ VX12 = i_vertex3d[1].z;\r
+ VX20 = i_vertex3d[2].x;\r
+ VX21 = i_vertex3d[2].y;\r
+ VX22 = i_vertex3d[2].z;\r
+ VX30 = i_vertex3d[3].x;\r
+ VX31 = i_vertex3d[3].y;\r
+ VX32 = i_vertex3d[3].z;\r
+ final long P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31;\r
+ P2D00 = i_vertex2d[0].x;\r
+ P2D01 = i_vertex2d[0].y;\r
+ P2D10 = i_vertex2d[1].x;\r
+ P2D11 = i_vertex2d[1].y;\r
+ P2D20 = i_vertex2d[2].x;\r
+ P2D21 = i_vertex2d[2].y;\r
+ P2D30 = i_vertex2d[3].x;\r
+ P2D31 = i_vertex2d[3].y;\r
+ final NyARPerspectiveProjectionMatrix prjmat = this._projection_mat_ref;\r
+ final long CP0,CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10;\r
+ CP0 = (long)(prjmat.m00*0x10000L);\r
+ CP1 = (long)(prjmat.m01*0x10000L);\r
+ CP2 = (long)(prjmat.m02*0x10000L);\r
+ CP4 = (long)(prjmat.m10*0x10000L);\r
+ CP5 = (long)(prjmat.m11*0x10000L);\r
+ CP6 = (long)(prjmat.m12*0x10000L);\r
+ CP8 = (long)(prjmat.m20*0x10000L);\r
+ CP9 = (long)(prjmat.m21*0x10000L);\r
+ CP10 =(long)(prjmat.m22*0x10000L);\r
+\r
+ combo03 = ((CP0 * trans.x + CP1 * trans.y + CP2 * trans.z)>>16) + (long)(prjmat.m03*0x10000L);\r
+ combo13 = ((CP4 * trans.x + CP5 * trans.y + CP6 * trans.z)>>16) + (long)(prjmat.m13*0x10000L);\r
+ combo23 = ((CP8 * trans.x + CP9 * trans.y + CP10 * trans.z)>>16) + (long)(prjmat.m23*0x10000L);\r
+ long CACA, SASA, SACA, CA, SA;\r
+ long CACACB, SACACB, SASACB, CASB, SASB;\r
+ long SACASC, SACACBSC, SACACBCC, SACACC;\r
+ final long[][] double1D = this.__modifyMatrix_double1D;\r
+\r
+ final NyARFixedFloat16Point3d angle = this.__modifyMatrix_angle;\r
+ final long[] a_factor = double1D[1];\r
+ final long[] sinb = double1D[2];\r
+ final long[] cosb = double1D[3];\r
+ final long[] b_factor = double1D[4];\r
+ final long[] sinc = double1D[5];\r
+ final long[] cosc = double1D[6];\r
+ final long[] c_factor = double1D[7];\r
+ long w, w2;\r
+ long wsin, wcos;\r
+ \r
+ io_rot.getAngle(angle);// arGetAngle( rot, &a, &b, &c );\r
+ a2 =angle.x;\r
+ b2 =angle.y;\r
+ c2 =angle.z;\r
+\r
+ // comboの3行目を先に計算\r
+ for (int i = 0; i < 10; i++) {\r
+ minerr = 0x4000000000000000L;\r
+ // sin-cosテーブルを計算(これが外に出せるとは…。)\r
+ for (int j = 0; j < 3; j++) {\r
+ w2 = factor * (j - 1);//S16\r
+ w = a2 + w2;//S16\r
+ a_factor[j] = w;//S16\r
+ w = b2 + w2;//S16\r
+ b_factor[j] = w;//S16\r
+ sinb[j] = NyMath.sinFixedFloat24((int)w);\r
+ cosb[j] = NyMath.cosFixedFloat24((int)w);\r
+ w = c2 + w2;//S16\r
+ c_factor[j] = w;//S16\r
+ sinc[j] = NyMath.sinFixedFloat24((int)w);\r
+ cosc[j] = NyMath.cosFixedFloat24((int)w);\r
+ }\r
+ //\r
+ for (t1 = 0; t1 < 3; t1++) {\r
+ SA = NyMath.sinFixedFloat24((int)a_factor[t1]);\r
+ CA = NyMath.cosFixedFloat24((int)a_factor[t1]);\r
+ // Optimize\r
+ CACA = (CA * CA)>>24;//S24\r
+ SASA = (SA * SA)>>24;//S24\r
+ SACA = (SA * CA)>>24;//S24\r
+ for (t2 = 0; t2 < 3; t2++) {\r
+ wsin = sinb[t2];//S24\r
+ wcos = cosb[t2];//S24\r
+ CACACB = (CACA * wcos)>>24;//S24\r
+ SACACB = (SACA * wcos)>>24;//S24\r
+ SASACB = (SASA * wcos)>>24;//S24\r
+ CASB = (CA * wsin)>>24;//S24\r
+ SASB = (SA * wsin)>>24;//S24\r
+\r
+ // comboの計算1\r
+ combo02 = (CP0 * CASB + CP1 * SASB + CP2 * wcos)>>24;//S24*S16>>24=S16\r
+ combo12 = (CP4 * CASB + CP5 * SASB + CP6 * wcos)>>24;//S24*S16>>24=S16\r
+ combo22 = (CP8 * CASB + CP9 * SASB + CP10 * wcos)>>24;//S24*S16>>24=S16\r
+\r
+ combo02_2 = ((combo02 * VX02)>>16) + combo03;//S16\r
+ combo02_5 = ((combo02 * VX12)>>16) + combo03;//S16\r
+ combo02_8 = ((combo02 * VX22)>>16) + combo03;//S16\r
+ combo02_11 = ((combo02 * VX32)>>16) + combo03;//S16\r
+ combo12_2 = ((combo12 * VX02)>>16) + combo13;//S16\r
+ combo12_5 = ((combo12 * VX12)>>16) + combo13;//S16\r
+ combo12_8 = ((combo12 * VX22)>>16) + combo13;//S16\r
+ combo12_11 = ((combo12 * VX32)>>16) + combo13;//S16\r
+ combo22_2 = ((combo22 * VX02)>>16) + combo23;//S16\r
+ combo22_5 = ((combo22 * VX12)>>16) + combo23;//S16\r
+ combo22_8 = ((combo22 * VX22)>>16) + combo23;//S16\r
+ combo22_11 = ((combo22 * VX32)>>16) + combo23;//S16\r
+ for (t3 = 0; t3 < 3; t3++) {\r
+ wsin = sinc[t3];//S24\r
+ wcos = cosc[t3];//S24\r
+ SACASC = (SACA * wsin)>>24;//S24\r
+ SACACC = (SACA * wcos)>>24;//S24\r
+ SACACBSC =(SACACB * wsin)>>24;//S24;\r
+ SACACBCC = (SACACB * wcos)>>24;//S24;\r
+\r
+ rot0 = ((CACACB * wcos + SASA * wcos)>>24) + SACACBSC - SACASC;//S24;\r
+ rot1 = SACACBCC - SACACC + ((SASACB * wsin + CACA * wsin)>>24);//S24;\r
+ rot2 = (-CASB * wcos - SASB * wsin)>>24;//S24;\r
+ combo00 = (CP0 * rot0 + CP1 * rot1 + CP2 * rot2)>>24;//S16\r
+ combo10 = (CP4 * rot0 + CP5 * rot1 + CP6 * rot2)>>24;//S16\r
+ combo20 = (CP8 * rot0 + CP9 * rot1 + CP10 * rot2)>>24;//S16\r
+\r
+ rot0 = ((-CACACB * wsin - SASA * wsin)>>24) + SACACBCC - SACACC;//S24\r
+ rot1 = -SACACBSC + SACASC + ((SASACB * wcos + CACA * wcos)>>24);//S24\r
+ rot2 = (CASB * wsin - SASB * wcos)>>24;//S24\r
+\r
+ combo01 =(CP0 * rot0 + CP1 * rot1 + CP2 * rot2)>>24;//S16\r
+ combo11 =(CP4 * rot0 + CP5 * rot1 + CP6 * rot2)>>24;//S16\r
+ combo21 =(CP8 * rot0 + CP9 * rot1 + CP10 * rot2)>>24;//S16\r
+ //\r
+ err =0;\r
+ h = ((combo20 * VX00 + combo21 * VX01)>>16) + combo22_2;//S16\r
+ x = P2D00 - ((((combo00 * VX00 + combo01 * VX01)>>16) + combo02_2)<<16) / h;//S16\r
+ y = P2D01 - ((((combo10 * VX00 + combo11 * VX01)>>16) + combo12_2)<<16) / h;//S16\r
+ err += ((x * x + y * y)>>16);\r
+ h = ((combo20 * VX10 + combo21 * VX11)>>16) + combo22_5;\r
+ x = P2D10 - ((((combo00 * VX10 + combo01 * VX11)>>16) + combo02_5)<<16) / h;//S16\r
+ y = P2D11 - ((((combo10 * VX10 + combo11 * VX11)>>16) + combo12_5)<<16) / h;//S16\r
+ err += ((x * x + y * y)>>16);\r
+ h = ((combo20 * VX20 + combo21 * VX21)>>16) + combo22_8;\r
+ x = P2D20 - ((((combo00 * VX20 + combo01 * VX21)>>16) + combo02_8)<<16) / h;//S16\r
+ y = P2D21 - ((((combo10 * VX20 + combo11 * VX21)>>16) + combo12_8)<<16) / h;//S16\r
+ err += ((x * x + y * y)>>16);\r
+ h = ((combo20 * VX30 + combo21 * VX31)>>16) + combo22_11;\r
+ x = P2D30 - ((((combo00 * VX30 + combo01 * VX31)>>16) + combo02_11)<<16) / h;//S16\r
+ y = P2D31 - ((((combo10 * VX30 + combo11 * VX31)>>16) + combo12_11)<<16) / h;//S16\r
+ err += ((x * x + y * y)>>16);\r
+ if (err < minerr) {\r
+ minerr = err;\r
+ a2 = a_factor[t1];\r
+ b2 = b_factor[t2];\r
+ c2 = c_factor[t3];\r
+ best_idx=t1+t2*3+t3*9;\r
+ }\r
+ }\r
+ }\r
+ }\r
+ if (best_idx==(1+3+9)) {\r
+ factor=factor>>1;\r
+ }\r
+ }\r
+ io_rot.setAngle((int)a2,(int)b2,(int)c2);\r
+ /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
+ return minerr /4;\r
+ } \r
+ \r
+ \r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.NyARMat;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core2.types.matrix.NyARFixedFloat16Matrix33;\r
+\r
+public class NyARFixedFloatRotVector\r
+{\r
+\r
+ //publicメンバ達\r
+ public double v1;\r
+\r
+ public double v2;\r
+\r
+ public double v3;\r
+\r
+ //privateメンバ達\r
+ private NyARFixedFloat16Matrix33 _cmat=new NyARFixedFloat16Matrix33();\r
+ \r
+ private NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
+\r
+ private double[][] _inv_cpara_array_ref;\r
+\r
+ public NyARFixedFloatRotVector(NyARPerspectiveProjectionMatrix i_cmat) throws NyARException\r
+ {\r
+ NyARMat mat_a = new NyARMat(3, 3);\r
+ double[][] a_array = mat_a.getArray();\r
+ \r
+ a_array[0][0] =i_cmat.m00;\r
+ a_array[0][1] =i_cmat.m01;\r
+ a_array[0][2] =i_cmat.m02;\r
+ a_array[1][0] =i_cmat.m10;\r
+ a_array[1][1] =i_cmat.m11;\r
+ a_array[1][2] =i_cmat.m12;\r
+ a_array[2][0] =i_cmat.m20;\r
+ a_array[2][1] =i_cmat.m21;\r
+ a_array[2][2] =i_cmat.m22;\r
+ \r
+ mat_a.matrixSelfInv();\r
+ this._projection_mat_ref = i_cmat;\r
+ //FixedFloat16にコピー\r
+ this._cmat.m00=(long)(i_cmat.m00*0x10000);\r
+ this._cmat.m01=(long)(i_cmat.m01*0x10000);\r
+ this._cmat.m02=(long)(i_cmat.m02*0x10000);\r
+ this._cmat.m10=(long)(i_cmat.m10*0x10000);\r
+ this._cmat.m11=(long)(i_cmat.m11*0x10000);\r
+ this._cmat.m12=(long)(i_cmat.m12*0x10000);\r
+ this._cmat.m20=(long)(i_cmat.m20*0x10000);\r
+ this._cmat.m21=(long)(i_cmat.m21*0x10000);\r
+ this._cmat.m22=(long)(i_cmat.m22*0x10000);\r
+ this._inv_cpara_array_ref = mat_a.getArray();\r
+ //GCない言語のときは、ここで配列の所有権委譲してね！\r
+ }\r
+\r
+ /**\r
+ * ２直線に直交するベクトルを計算する・・・だと思う。\r
+ * @param i_linear1\r
+ * @param i_linear2\r
+ */\r
+ public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2)\r
+ {\r
+ //1行目\r
+ final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref;\r
+ final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise;\r
+ final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept;\r
+ final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run;\r
+\r
+ final double m0 = w1 * (cmat.m01 * cmat.m12 - cmat.m02 * cmat.m11) + w2 * cmat.m11 - w3 * cmat.m01;//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];\r
+ final double m1 = -w1 * cmat.m00 * cmat.m12 + w3 * cmat.m00;//-w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
+ final double m2 = w1 * cmat.m00 * cmat.m11;//w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
+ final double w = Math.sqrt(m0 * m0 + m1 * m1 + m2 * m2);\r
+ this.v1 = m0 / w;\r
+ this.v2 = m1 / w;\r
+ this.v3 = m2 / w;\r
+ return;\r
+ }\r
+\r
+ /**\r
+ * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) Optimize:STEP[526->468]\r
+ * ベクトルの開始/終了座標を指定して、ベクトルの方向を調整する。\r
+ * @param i_start_vertex\r
+ * @param i_end_vertex\r
+ * @param cpara\r
+ */\r
+ public void checkVectorByVertex(final NyARDoublePoint2d i_start_vertex, final NyARDoublePoint2d i_end_vertex) throws NyARException\r
+ {\r
+ double h;\r
+ final double[][] inv_cpara = this._inv_cpara_array_ref;\r
+ //final double[] world = __checkVectorByVertex_world;// [2][3];\r
+ final double world0 = inv_cpara[0][0] * i_start_vertex.x * 10.0 + inv_cpara[0][1] * i_start_vertex.y * 10.0 + inv_cpara[0][2] * 10.0;// mat_a->m[0]*st[0]*10.0+\r
+ final double world1 = inv_cpara[1][0] * i_start_vertex.x * 10.0 + inv_cpara[1][1] * i_start_vertex.y * 10.0 + inv_cpara[1][2] * 10.0;// mat_a->m[3]*st[0]*10.0+\r
+ final double world2 = inv_cpara[2][0] * i_start_vertex.x * 10.0 + inv_cpara[2][1] * i_start_vertex.y * 10.0 + inv_cpara[2][2] * 10.0;// mat_a->m[6]*st[0]*10.0+\r
+ final double world3 = world0 + this.v1;\r
+ final double world4 = world1 + this.v2;\r
+ final double world5 = world2 + this.v3;\r
+ // </Optimize>\r
+\r
+ //final double[] camera = __checkVectorByVertex_camera;// [2][2];\r
+ final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref;\r
+ //h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * world2;\r
+ h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;\r
+ if (h == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ //final double camera0 = (cpara[0 * 4 + 0] * world0 + cpara[0 * 4 + 1] * world1 + cpara[0 * 4 + 2] * world2) / h;\r
+ //final double camera1 = (cpara[1 * 4 + 0] * world0 + cpara[1 * 4 + 1] * world1 + cpara[1 * 4 + 2] * world2) / h;\r
+ final double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;\r
+ final double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;\r
+\r
+ //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;\r
+ h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;\r
+ if (h == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ //final double camera2 = (cpara[0 * 4 + 0] * world3 + cpara[0 * 4 + 1] * world4 + cpara[0 * 4 + 2] * world5) / h;\r
+ //final double camera3 = (cpara[1 * 4 + 0] * world3 + cpara[1 * 4 + 1] * world4 + cpara[1 * 4 + 2] * world5) / h;\r
+ final double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;\r
+ final double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;\r
+\r
+ final double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);\r
+ if (v < 0) {\r
+ this.v1 = -this.v1;\r
+ this.v2 = -this.v2;\r
+ this.v3 = -this.v3;\r
+ }\r
+ }\r
+ /**\r
+ * int check_rotation( double rot[2][3] )\r
+ * 2つのベクトル引数の調整をする？\r
+ * @param i_r\r
+ * @throws NyARException\r
+ */\r
+\r
+ public final static void checkRotation(NyARFixedFloatRotVector io_vec1, NyARFixedFloatRotVector io_vec2) throws NyARException\r
+ {\r
+ double w;\r
+ int f;\r
+\r
+ double vec10 = io_vec1.v1;\r
+ double vec11 = io_vec1.v2;\r
+ double vec12 = io_vec1.v3;\r
+ double vec20 = io_vec2.v1;\r
+ double vec21 = io_vec2.v2;\r
+ double vec22 = io_vec2.v3;\r
+ \r
+ double vec30 = vec11 * vec22 - vec12 * vec21;\r
+ double vec31 = vec12 * vec20 - vec10 * vec22;\r
+ double vec32 = vec10 * vec21 - vec11 * vec20;\r
+ w = Math.sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);\r
+ if (w == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ vec30 /= w;\r
+ vec31 /= w;\r
+ vec32 /= w;\r
+\r
+ double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;\r
+ if (cb < 0){\r
+ cb=-cb;//cb *= -1.0; \r
+ }\r
+ final double ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5;\r
+\r
+ if (vec31 * vec10 - vec11 * vec30 != 0.0) {\r
+ f = 0;\r
+ } else {\r
+ if (vec32 * vec10 - vec12 * vec30 != 0.0) {\r
+ w = vec11;vec11 = vec12;vec12 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 1;\r
+ } else {\r
+ w = vec10;vec10 = vec12;vec12 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 2;\r
+ }\r
+ }\r
+ if (vec31 * vec10 - vec11 * vec30 == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ \r
+ double k1,k2,k3,k4;\r
+ double a, b, c, d;\r
+ double p1, q1, r1;\r
+ double p2, q2, r2;\r
+ double p3, q3, r3;\r
+ double p4, q4, r4; \r
+ \r
+ \r
+ k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);\r
+ k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);\r
+ k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);\r
+ k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);\r
+\r
+ a = k1 * k1 + k3 * k3 + 1;\r
+ b = k1 * k2 + k3 * k4;\r
+ c = k2 * k2 + k4 * k4 - 1;\r
+\r
+ d = b * b - a * c;\r
+ if (d < 0) {\r
+ throw new NyARException();\r
+ }\r
+ r1 = (-b + Math.sqrt(d)) / a;\r
+ p1 = k1 * r1 + k2;\r
+ q1 = k3 * r1 + k4;\r
+ r2 = (-b - Math.sqrt(d)) / a;\r
+ p2 = k1 * r2 + k2;\r
+ q2 = k3 * r2 + k4;\r
+ if (f == 1) {\r
+ w = q1;q1 = r1;r1 = w;\r
+ w = q2;q2 = r2;r2 = w;\r
+ w = vec11;vec11 = vec12;vec12 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+ if (f == 2) {\r
+ w = p1;p1 = r1;r1 = w;\r
+ w = p2;p2 = r2;r2 = w;\r
+ w = vec10;vec10 = vec12;vec12 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+\r
+ if (vec31 * vec20 - vec21 * vec30 != 0.0) {\r
+ f = 0;\r
+ } else {\r
+ if (vec32 * vec20 - vec22 * vec30 != 0.0) {\r
+ w = vec21;vec21 = vec22;vec22 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 1;\r
+ } else {\r
+ w = vec20;vec20 = vec22;vec22 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 2;\r
+ }\r
+ }\r
+ if (vec31 * vec20 - vec21 * vec30 == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);\r
+ k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);\r
+ k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);\r
+ k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);\r
+\r
+ a = k1 * k1 + k3 * k3 + 1;\r
+ b = k1 * k2 + k3 * k4;\r
+ c = k2 * k2 + k4 * k4 - 1;\r
+\r
+ d = b * b - a * c;\r
+ if (d < 0) {\r
+ throw new NyARException();\r
+ }\r
+ r3 = (-b + Math.sqrt(d)) / a;\r
+ p3 = k1 * r3 + k2;\r
+ q3 = k3 * r3 + k4;\r
+ r4 = (-b - Math.sqrt(d)) / a;\r
+ p4 = k1 * r4 + k2;\r
+ q4 = k3 * r4 + k4;\r
+ if (f == 1) {\r
+ w = q3;q3 = r3;r3 = w;\r
+ w = q4;q4 = r4;r4 = w;\r
+ w = vec21;vec21 = vec22;vec22 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+ if (f == 2) {\r
+ w = p3;p3 = r3;r3 = w;\r
+ w = p4;p4 = r4;r4 = w;\r
+ w = vec20;vec20 = vec22;vec22 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+\r
+ double e1 = p1 * p3 + q1 * q3 + r1 * r3;\r
+ if (e1 < 0) {\r
+ e1 = -e1;\r
+ }\r
+ double e2 = p1 * p4 + q1 * q4 + r1 * r4;\r
+ if (e2 < 0) {\r
+ e2 = -e2;\r
+ }\r
+ double e3 = p2 * p3 + q2 * q3 + r2 * r3;\r
+ if (e3 < 0) {\r
+ e3 = -e3;\r
+ }\r
+ double e4 = p2 * p4 + q2 * q4 + r2 * r4;\r
+ if (e4 < 0) {\r
+ e4 = -e4;\r
+ }\r
+ if (e1 < e2) {\r
+ if (e1 < e3) {\r
+ if (e1 < e4) {\r
+ io_vec1.v1 = p1;\r
+ io_vec1.v2 = q1;\r
+ io_vec1.v3 = r1;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ } else {\r
+ if (e3 < e4) {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ } else {\r
+ if (e2 < e3) {\r
+ if (e2 < e4) {\r
+ io_vec1.v1 = p1;\r
+ io_vec1.v2 = q1;\r
+ io_vec1.v3 = r1;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ } else {\r
+ if (e3 < e4) {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ }\r
+ return;\r
+ } \r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d;\r
+\r
+\r
+final public class NyARFixedFloatTransOffset\r
+{\r
+ public NyARFixedFloat16Point3d[] vertex=NyARFixedFloat16Point3d.createArray(4);\r
+ public NyARFixedFloat16Point3d point=new NyARFixedFloat16Point3d(); \r
+ /**\r
+ * 中心位置と辺長から、オフセット情報を作成して設定する。\r
+ * @param i_width\r
+ * FF16で渡すこと！\r
+ * @param i_center\r
+ */\r
+ public void setSquare(long i_width,NyARFixedFloat16Point2d i_center)\r
+ {\r
+ final long w_2 = i_width>>1;\r
+ \r
+ NyARFixedFloat16Point3d vertex3d_ptr;\r
+ vertex3d_ptr= this.vertex[0];\r
+ vertex3d_ptr.x = -w_2;\r
+ vertex3d_ptr.y = w_2;\r
+ vertex3d_ptr.z = 0;\r
+ vertex3d_ptr= this.vertex[1];\r
+ vertex3d_ptr.x = w_2;\r
+ vertex3d_ptr.y = w_2;\r
+ vertex3d_ptr.z = 0;\r
+ vertex3d_ptr= this.vertex[2];\r
+ vertex3d_ptr.x = w_2;\r
+ vertex3d_ptr.y = -w_2;\r
+ vertex3d_ptr.z = 0;\r
+ vertex3d_ptr= this.vertex[3];\r
+ vertex3d_ptr.x = -w_2;\r
+ vertex3d_ptr.y = -w_2;\r
+ vertex3d_ptr.z = 0;\r
+ \r
+ this.point.x=-i_center.x;\r
+ this.point.y=-i_center.y;\r
+ this.point.z=0;\r
+ return;\r
+ }\r
+}\r
+++ /dev/null
-/* \r
- * PROJECT: NyARToolkit\r
- * --------------------------------------------------------------------------------\r
- * This work is based on the original ARToolKit developed by\r
- * Hirokazu Kato\r
- * Mark Billinghurst\r
- * HITLab, University of Washington, Seattle\r
- * http://www.hitl.washington.edu/artoolkit/\r
- *\r
- * The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
- *\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- * \r
- * For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
- * <airmail(at)ebony.plala.or.jp>\r
- * \r
- */\r
-package jp.nyatla.nyartoolkit.sandbox.x2;\r
-\r
-import jp.nyatla.nyartoolkit.NyARException;\r
-import jp.nyatla.nyartoolkit.core.param.NyARCameraDistortionFactor;\r
-import jp.nyatla.nyartoolkit.core.types.*;\r
-import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
-import jp.nyatla.nyartoolkit.core.pca2d.*;\r
-/**\r
- * 線分から任意個数の点をサンプリングして、それに対するPCAを実行する関数\r
- * このクラスでは、線分の両端からn/2個の点を抽出する。\r
- *\r
- */\r
-public class NyARPca2d_SamplingPCA implements INyARPca2d\r
-{\r
- private double[] _x;\r
- private double[] _y;\r
- private int[] _tmp_x;\r
- private int[] _tmp_y;\r
-\r
- private int _number_of_data;\r
- private int _sampling_points;\r
-\r
- private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
-\r
- private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\r
-\r
- private static final double PCA_VZERO = 1e-16; // #define VZERO 1e-16\r
- public NyARPca2d_SamplingPCA(int i_sampling_points)\r
- {\r
- this._x=new double[i_sampling_points];\r
- this._y=new double[i_sampling_points];\r
- //サンプリング用配列\r
- this._tmp_x=new int[i_sampling_points];\r
- this._tmp_y=new int[i_sampling_points];\r
- this._sampling_points=i_sampling_points;\r
- this._number_of_data=0;\r
- return;\r
- }\r
-\r
-\r
- /**\r
- * static int QRM( ARMat *a, ARVec *dv )の代替関数\r
- * \r
- * @param a\r
- * @param dv\r
- * @throws NyARException\r
- */\r
- private static void PCA_QRM(NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d dv) throws NyARException\r
- {\r
- double w, t, s, x, y, c;\r
- double ev1;\r
- double dv_x,dv_y;\r
- double mat00,mat01,mat10,mat11;\r
- // <this.vecTridiagonalize2d(i_mat, dv, ev);>\r
- dv_x = o_matrix.m00;// 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)];\r
- ev1 = o_matrix.m01;// 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)];\r
- dv_y = o_matrix.m11;// 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)];\r
- // 単位行列にする。\r
- mat00 = mat11 = 1;\r
- mat01 = mat10 = 0;\r
- // </this.vecTridiagonalize2d(i_mat, dv, ev);>\r
-\r
- // int j = 1;\r
- // // while(j>0 && fabs(ev->v[j])>EPS*(fabs(dv->v[j-1])+fabs(dv->v[j])))\r
- // while (j > 0 && Math.abs(ev1) > PCA_EPS * (Math.abs(dv.x) + Math.abs(dv.y))) {\r
- // j--;\r
- // }\r
- // if (j == 0) {\r
- int iter = 0;\r
- do {\r
- iter++;\r
- if (iter > PCA_MAX_ITER) {\r
- break;\r
- }\r
- w = (dv_x - dv_y) / 2;// w = (dv->v[h-1] -dv->v[h]) / 2;//ここ？\r
- t = ev1 * ev1;// t = ev->v[h] * ev->v[h];\r
- s = Math.sqrt(w * w + t);\r
- if (w < 0) {\r
- s = -s;\r
- }\r
- x = dv_x - dv_y + t / (w + s);// x = dv->v[j] -dv->v[h] +t/(w+s);\r
- y = ev1;// y = ev->v[j+1];\r
-\r
- if (Math.abs(x) >= Math.abs(y)) {\r
- if (Math.abs(x) > PCA_VZERO) {\r
- t = -y / x;\r
- c = 1 / Math.sqrt(t * t + 1);\r
- s = t * c;\r
- } else {\r
- c = 1.0;\r
- s = 0.0;\r
- }\r
- } else {\r
- t = -x / y;\r
- s = 1.0 / Math.sqrt(t * t + 1);\r
- c = t * s;\r
- }\r
- w = dv_x - dv_y;// w = dv->v[k] -dv->v[k+1];\r
- t = (w * s + 2 * c * ev1) * s;// t = (w * s +2 * c *ev->v[k+1]) *s;\r
- dv_x -= t;// dv->v[k] -= t;\r
- dv_y += t;// dv->v[k+1] += t;\r
- ev1 += s * (c * w - 2 * s * ev1);// ev->v[k+1]+= s * (c* w- 2* s *ev->v[k+1]);\r
-\r
- x = mat00;// x = a->m[k*dim+i];\r
- y = mat10;// y = a->m[(k+1)*dim+i];\r
- mat00 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
- mat10 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
- \r
- x = mat01;// x = a->m[k*dim+i];\r
- y = mat11;// y = a->m[(k+1)*dim+i];\r
- mat01 = c * x - s * y;// a->m[k*dim+i] = c * x - s* y;\r
- mat11 = s * x + c * y;// a->m[(k+1)*dim+i] = s* x + c * y;\r
- } while (Math.abs(ev1) > PCA_EPS * (Math.abs(dv_x) + Math.abs(dv_y)));\r
- // }\r
-\r
- t = dv_x;// t = dv->v[h];\r
- if (dv_y > t) {// if( dv->v[i] > t ) {\r
- t = dv_y;// t = dv->v[h];\r
- dv_y = dv_x;// dv->v[h] = dv->v[k];\r
- dv_x = t;// dv->v[k] = t;\r
- // 行の入れ替え\r
- o_matrix.m00 = mat10;\r
- o_matrix.m01 = mat11;\r
- o_matrix.m10 = mat00; \r
- o_matrix.m11 = mat01;\r
- \r
- } else {\r
- // 行の入れ替えはなし\r
- o_matrix.m00 = mat00;\r
- o_matrix.m01 = mat01;\r
- o_matrix.m10 = mat10; \r
- o_matrix.m11 = mat11;\r
- }\r
- dv.x=dv_x;\r
- dv.y=dv_y;\r
- return;\r
- }\r
-\r
- /**\r
- * static int PCA( ARMat *input, ARMat *output, ARVec *ev )\r
- * \r
- * @param output\r
- * @param o_ev\r
- * @throws NyARException\r
- */\r
- private void PCA_PCA(NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
- {\r
- // double[] mean_array=mean.getArray();\r
- // mean.zeroClear();\r
-\r
- //PCA_EXの処理\r
- double sx = 0;\r
- double sy = 0;\r
- final int number_of_data = this._number_of_data;\r
- for (int i = 0; i < number_of_data; i++) {\r
- sx += this._x[i];\r
- sy += this._y[i];\r
- }\r
- sx = sx / number_of_data;\r
- sy = sy / number_of_data;\r
- \r
- //PCA_CENTERとPCA_xt_by_xを一緒に処理\r
- final double srow = Math.sqrt((double) this._number_of_data);\r
- double w00, w11, w10;\r
- w00 = w11 = w10 = 0.0;// *out = 0.0;\r
- for (int i = 0; i < number_of_data; i++) {\r
- final double x = (this._x[i] - sx) / srow;\r
- final double y = (this._y[i] - sy) / srow;\r
- w00 += (x * x);// *out += *in1 * *in2;\r
- w10 += (x * y);// *out += *in1 * *in2;\r
- w11 += (y * y);// *out += *in1 * *in2;\r
- }\r
- o_matrix.m00=w00;\r
- o_matrix.m01=o_matrix.m10=w10;\r
- o_matrix.m11=w11;\r
- \r
- //PCA_PCAの処理\r
- PCA_QRM(o_matrix, o_ev);\r
- // m2 = o_output.m;// m2 = output->m;\r
- if (o_ev.x < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
- o_ev.x = 0.0;// ev->v[i] = 0.0;\r
- o_matrix.m00 = 0.0;// *(m2++) = 0.0;\r
- o_matrix.m01 = 0.0;// *(m2++) = 0.0;\r
- }\r
-\r
- if (o_ev.y < PCA_VZERO) {// if( ev->v[i] < VZERO ){\r
- o_ev.y = 0.0;// ev->v[i] = 0.0;\r
- o_matrix.m10 = 0.0;// *(m2++) = 0.0;\r
- o_matrix.m11 = 0.0;// *(m2++) = 0.0;\r
- }\r
- o_mean.x=sx;\r
- o_mean.y=sy;\r
- // }\r
- return;\r
- }\r
- public void pca(double[] i_x,double[] i_y,int i_start,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
- {\r
- NyARException.trap("未チェックの関数");\r
- if(i_number_of_point<this._sampling_points){\r
- this._number_of_data = i_number_of_point;\r
- System.arraycopy(i_x, 0, this._x, i_start, i_number_of_point);\r
- System.arraycopy(i_y, 0, this._y, i_start, i_number_of_point);\r
- }else{\r
- //サンプリング個数分の点を、両端から半分づつ取ってくる。\r
- int st=i_start;\r
- int ed=i_start+i_number_of_point-1;\r
- for(int i=this._sampling_points-1;i>=0;i+=2){\r
- this._x[i]=i_x[st];\r
- this._y[i]=i_y[st];\r
- this._x[i+1]=i_x[ed];\r
- this._y[i+1]=i_y[ed];\r
- ed--;\r
- st++;\r
- }\r
- this._number_of_data = this._sampling_points; \r
- }\r
- PCA_PCA(o_evec, o_ev,o_mean);\r
-\r
- final double sum = o_ev.x + o_ev.y;\r
- // For順変更禁止\r
- o_ev.x /= sum;// ev->v[i] /= sum;\r
- o_ev.y /= sum;// ev->v[i] /= sum;\r
- return; \r
- }\r
- \r
- public void pcaWithDistortionFactor(int[] i_x,int[] i_y,int i_start,int i_number_of_point,NyARCameraDistortionFactor i_factor,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
- {\r
- if(i_number_of_point<this._sampling_points){\r
- i_factor.observ2IdealBatch(i_x, i_y, i_start, i_number_of_point, this._x, this._y);\r
- this._number_of_data = i_number_of_point;\r
- }else{\r
- //サンプリング個数分の点を、両端から半分づつ取ってくる。\r
- int st=i_start;\r
- int ed=i_start+i_number_of_point-1;\r
- for(int i=this._sampling_points-1;i>=0;i-=2){\r
- this._tmp_x[i]=i_x[st];\r
- this._tmp_y[i]=i_y[st];\r
- this._tmp_x[i-1]=i_x[ed];\r
- this._tmp_y[i-1]=i_y[ed];\r
- ed--;\r
- st++;\r
- }\r
- i_factor.observ2IdealBatch(this._tmp_x,this._tmp_y,0,this._sampling_points, this._x, this._y);\r
- this._number_of_data = this._sampling_points; \r
- }\r
-\r
- PCA_PCA(o_evec,o_ev,o_mean);\r
-\r
- final double sum = o_ev.x + o_ev.y;\r
- // For順変更禁止\r
- o_ev.x /= sum;// ev->v[i] /= sum;\r
- o_ev.y /= sum;// ev->v[i] /= sum;\r
- return; \r
- }\r
-}\r
+++ /dev/null
-/* \r
- * PROJECT: NyARToolkit\r
- * --------------------------------------------------------------------------------\r
- * This work is based on the original ARToolKit developed by\r
- * Hirokazu Kato\r
- * Mark Billinghurst\r
- * HITLab, University of Washington, Seattle\r
- * http://www.hitl.washington.edu/artoolkit/\r
- *\r
- * The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
- *\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- * \r
- * For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
- * <airmail(at)ebony.plala.or.jp>\r
- * \r
- */\r
-package jp.nyatla.nyartoolkit.sandbox.x2;\r
-\r
-import jp.nyatla.nyartoolkit.NyARException;\r
-import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult;\r
-import jp.nyatla.nyartoolkit.core.types.*;\r
-import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*;\r
-import jp.nyatla.nyartoolkit.core.param.*;\r
-import jp.nyatla.nyartoolkit.sandbox.x2.NyARSinTable;\r
-\r
-\r
-\r
-/**\r
- * 回転行列計算用の、3x3行列\r
- * \r
- */\r
-public class NyARRotMatrix_X2 extends NyARRotMatrix\r
-{\r
- private NyARSinTable _sin_table_ref;\r
-\r
- /**\r
- * インスタンスを準備します。\r
- * \r
- * @param i_param\r
- */\r
- public NyARRotMatrix_X2(NyARPerspectiveProjectionMatrix i_matrix,NyARSinTable i_sin_table_ref) throws NyARException\r
- {\r
- this.__initRot_vec1 = new NyARRotVector(i_matrix);\r
- this.__initRot_vec2 = new NyARRotVector(i_matrix);\r
- this._sin_table_ref=i_sin_table_ref;\r
- return;\r
- }\r
-\r
- final private NyARRotVector __initRot_vec1;\r
-\r
- final private NyARRotVector __initRot_vec2;\r
-\r
- public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
- {\r
-\r
- this.m00 = i_prev_result.m00;\r
- this.m01 = i_prev_result.m01;\r
- this.m02 = i_prev_result.m02;\r
-\r
- this.m10 = i_prev_result.m10;\r
- this.m11 = i_prev_result.m11;\r
- this.m12 = i_prev_result.m12;\r
-\r
- this.m20 = i_prev_result.m20;\r
- this.m21 = i_prev_result.m21;\r
- this.m22 = i_prev_result.m22;\r
- return;\r
- }\r
-\r
- public final void initRotBySquare(final NyARLinear[] i_linear, final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
- {\r
- final NyARRotVector vec1 = this.__initRot_vec1;\r
- final NyARRotVector vec2 = this.__initRot_vec2;\r
-\r
- // 向かい合った辺から、２本のベクトルを計算\r
-\r
- // 軸１\r
- vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);\r
- vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);\r
-\r
- // 軸２\r
- vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);\r
- vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);\r
-\r
- // 回転の最適化？\r
- NyARRotVector.checkRotation(vec1, vec2);\r
-\r
- this.m00 = vec1.v1;\r
- this.m10 = vec1.v2;\r
- this.m20 = vec1.v3;\r
- this.m01 = vec2.v1;\r
- this.m11 = vec2.v2;\r
- this.m21 = vec2.v3;\r
-\r
- // 最後の軸を計算\r
- final double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;\r
- final double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;\r
- final double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;\r
- final double w = Math.sqrt(w02 * w02 + w12 * w12 + w22 * w22);\r
- this.m02 = w02 / w;\r
- this.m12 = w12 / w;\r
- this.m22 = w22 / w;\r
- return;\r
- }\r
-\r
- /**\r
- * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) Optimize:2008.04.20:STEP[481→433] 3x3変換行列から、回転角を復元して返します。\r
- * \r
- * @param o_angle\r
- * @return\r
- */\r
- public final void getAngle(final NyARDoublePoint3d o_angle)\r
- {\r
- double a, b, c;\r
- double sina, cosa, sinb, cosb, sinc, cosc;\r
-\r
- if (this.m22 > 1.0) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
- this.m22 = 1.0;// <Optimize/>rot[2][2] = 1.0;\r
- } else if (this.m22 < -1.0) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
- this.m22 = -1.0;// <Optimize/>rot[2][2] = -1.0;\r
- }\r
- cosb = this.m22;// <Optimize/>cosb = rot[2][2];\r
- b = Math.acos(cosb);\r
- sinb = this._sin_table_ref.sin(b);\r
- final double rot02 = this.m02;\r
- final double rot12 = this.m12;\r
- if (b >= 0.000001 || b <= -0.000001) {\r
- cosa = rot02 / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
- sina = rot12 / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
- if (cosa > 1.0) {\r
- /* printf("cos(alph) = %f\n", cosa); */\r
- cosa = 1.0;\r
- sina = 0.0;\r
- }\r
- if (cosa < -1.0) {\r
- /* printf("cos(alph) = %f\n", cosa); */\r
- cosa = -1.0;\r
- sina = 0.0;\r
- }\r
- if (sina > 1.0) {\r
- /* printf("sin(alph) = %f\n", sina); */\r
- sina = 1.0;\r
- cosa = 0.0;\r
- }\r
- if (sina < -1.0) {\r
- /* printf("sin(alph) = %f\n", sina); */\r
- sina = -1.0;\r
- cosa = 0.0;\r
- }\r
- a = Math.acos(cosa);\r
- if (sina < 0) {\r
- a = -a;\r
- }\r
- // <Optimize>\r
- // 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]);\r
- // 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]);\r
- final double tmp = (rot02 * rot02 + rot12 * rot12);\r
- sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;\r
- cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;\r
- // </Optimize>\r
-\r
- if (cosc > 1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = 1.0;\r
- sinc = 0.0;\r
- }\r
- if (cosc < -1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = -1.0;\r
- sinc = 0.0;\r
- }\r
- if (sinc > 1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = 1.0;\r
- cosc = 0.0;\r
- }\r
- if (sinc < -1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = -1.0;\r
- cosc = 0.0;\r
- }\r
- c = Math.acos(cosc);\r
- if (sinc < 0) {\r
- c = -c;\r
- }\r
- } else {\r
- a = b = 0.0;\r
- cosa = cosb = 1.0;\r
- sina = sinb = 0.0;\r
- cosc = this.m00;// cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
- sinc = this.m01;// sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
- if (cosc > 1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = 1.0;\r
- sinc = 0.0;\r
- }\r
- if (cosc < -1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = -1.0;\r
- sinc = 0.0;\r
- }\r
- if (sinc > 1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = 1.0;\r
- cosc = 0.0;\r
- }\r
- if (sinc < -1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = -1.0;\r
- cosc = 0.0;\r
- }\r
- c = Math.acos(cosc);\r
- if (sinc < 0) {\r
- c = -c;\r
- }\r
- }\r
- o_angle.x = a;// wa.value=a;//*wa = a;\r
- o_angle.y = b;// wb.value=b;//*wb = b;\r
- o_angle.z = c;// wc.value=c;//*wc = c;\r
- return;\r
- }\r
-\r
- /**\r
- * 回転角から回転行列を計算してセットします。\r
- * \r
- * @param i_x\r
- * @param i_y\r
- * @param i_z\r
- */\r
- public final void setAngle(final double i_x, final double i_y, final double i_z)\r
- {\r
- /*\r
- * |cos(a) -sin(a) 0| |cos(b) 0 sin(b)| |cos(a-c) sin(a-c) 0| rot = |sin(a) cos(a) 0| |0 1 0 | |-sin(a-c) cos(a-c) 0| |0 0 1| |-sin(b) 0 cos(b)| |0 0 1|\r
- */\r
-\r
- double Sa, Sb, Ca, Cb, Sac, Cac, CaCb, SaCb;\r
- Sa = this._sin_table_ref.sin(i_x);\r
- Ca = this._sin_table_ref.cos(i_x);\r
- Sb = this._sin_table_ref.sin(i_y);\r
- Cb = this._sin_table_ref.cos(i_y);\r
- Sac = this._sin_table_ref.sin(i_x - i_z);\r
- Cac = this._sin_table_ref.cos(i_x - i_z);\r
- CaCb = Ca * Cb;\r
- SaCb = Sa * Cb;\r
-\r
- this.m00 = CaCb * Cac + Sa * Sac;\r
- this.m01 = CaCb * Sac - Sa * Cac;\r
- this.m02 = Ca * Sb;\r
- this.m10 = SaCb * Cac - Ca * Sac;\r
- this.m11 = SaCb * Sac + Ca * Cac;\r
- this.m12 = Sa * Sb;\r
- this.m20 = -Sb * Cac;\r
- this.m21 = -Sb * Sac;\r
- this.m22 = Cb;\r
-\r
- return;\r
- }\r
-\r
- /**\r
- * i_in_pointを変換行列で座標変換する。\r
- * \r
- * @param i_in_point\r
- * @param i_out_point\r
- */\r
- public final void getPoint3d(final NyARDoublePoint3d i_in_point, final NyARDoublePoint3d i_out_point)\r
- {\r
- final double x = i_in_point.x;\r
- final double y = i_in_point.y;\r
- final double z = i_in_point.z;\r
- i_out_point.x = this.m00 * x + this.m01 * y + this.m02 * z;\r
- i_out_point.y = this.m10 * x + this.m11 * y + this.m12 * z;\r
- i_out_point.z = this.m20 * x + this.m21 * y + this.m22 * z;\r
- return;\r
- }\r
-\r
- /**\r
- * 複数の頂点を一括して変換する\r
- * \r
- * @param i_in_point\r
- * @param i_out_point\r
- * @param i_number_of_vertex\r
- */\r
- public final void getPoint3dBatch(final NyARDoublePoint3d[] i_in_point, NyARDoublePoint3d[] i_out_point, int i_number_of_vertex)\r
- {\r
- for (int i = i_number_of_vertex - 1; i >= 0; i--) {\r
- final NyARDoublePoint3d out_ptr = i_out_point[i];\r
- final NyARDoublePoint3d in_ptr = i_in_point[i];\r
- final double x = in_ptr.x;\r
- final double y = in_ptr.y;\r
- final double z = in_ptr.z;\r
- out_ptr.x = this.m00 * x + this.m01 * y + this.m02 * z;\r
- out_ptr.y = this.m10 * x + this.m11 * y + this.m12 * z;\r
- out_ptr.z = this.m20 * x + this.m21 * y + this.m22 * z;\r
- }\r
- return;\r
- }\r
-}\r
+++ /dev/null
-/* \r
- * PROJECT: NyARToolkit\r
- * --------------------------------------------------------------------------------\r
- * This work is based on the original ARToolKit developed by\r
- * Hirokazu Kato\r
- * Mark Billinghurst\r
- * HITLab, University of Washington, Seattle\r
- * http://www.hitl.washington.edu/artoolkit/\r
- *\r
- * The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
- *\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- * \r
- * For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
- * <airmail(at)ebony.plala.or.jp>\r
- * \r
- */\r
-package jp.nyatla.nyartoolkit.sandbox.x2;\r
-/**\r
- * 単純Sinテーブル\r
- *\r
- */\r
-public class NyARSinTable\r
-{\r
- private double[] _table = null;\r
- private int _resolution;\r
-\r
- private void initTable(int i_resolution)\r
- {\r
- //解像度は4の倍数で無いとダメ\r
- assert(i_resolution%4==0);\r
- if (this._table == null) {\r
- this._table = new double[i_resolution];\r
- int d4 = i_resolution / 4;\r
- // テーブル初期化(0-2PIを0-1024に作成)\r
- for (int i = 1; i < i_resolution; i++) {\r
- this._table[i] = (Math.sin(2 * Math.PI * (double) i / (double) i_resolution));\r
- }\r
- this._table[0] = 0;\r
- this._table[d4 - 1] = 1;\r
- this._table[d4 * 2 - 1] = 0;\r
- this._table[d4 * 3 - 1] = -1;\r
- }\r
- return;\r
- }\r
-\r
- public NyARSinTable(int i_resolution)\r
- {\r
- initTable(i_resolution);\r
- this._resolution=i_resolution;\r
- return;\r
- }\r
-\r
- public double sin(double i_rad)\r
- {\r
- final int resolution=this._resolution;\r
- // 0～2PIを0～1024に変換\r
- int rad_index = (int) (i_rad * resolution / (2 * Math.PI));\r
- rad_index = rad_index % resolution;\r
- if (rad_index < 0) {\r
- rad_index += resolution;\r
- }\r
- // ここで0-1024にいる\r
- return this._table[rad_index];\r
- }\r
-\r
- public double cos(double i_rad)\r
- {\r
- final int resolution=this._resolution;\r
- // 0～Math.PI/2を 0～256の値空間に変換\r
- int rad_index = (int) (i_rad * resolution / (2 * Math.PI));\r
- // 90度ずらす\r
- rad_index = (rad_index + resolution / 4) % resolution;\r
- // 負の領域に居たら、+1024しておく\r
- if (rad_index < 0) {\r
- rad_index += resolution;\r
- }\r
- // ここで0-1024にいる\r
- return this._table[rad_index];\r
- }\r
- /**\r
- * ラジアン角度をテーブルの角度インデックス番号に変換する。\r
- * 角度インデックスは、0<=n<_resolutionの範囲の整数\r
- * @param i_rad\r
- * @return\r
- */\r
- public int rad2tableIndex(double i_rad)\r
- {\r
- final int resolution=this._resolution;\r
- int rad_index = (int) (i_rad * resolution / (2 * Math.PI));\r
- rad_index = rad_index % resolution;\r
- if (rad_index < 0) {\r
- rad_index += resolution;\r
- }\r
- return rad_index;\r
- }\r
- public double sinByIdx(int i_rad_idx)\r
- {\r
- return 0;\r
- }\r
- public double cosByIdx(int i_rad_idx)\r
- {\r
- return 0; \r
- }\r
- \r
-}\r
public NyARSingleDetectMarker_X2(NyARParam i_param, NyARCode i_code, double i_marker_width) throws NyARException\r
{\r
final NyARIntSize scr_size=i_param.getScreenSize(); \r
+ final NyARFixedFloatCameraDistortionFactorMap dist_factor_map=new NyARFixedFloatCameraDistortionFactorMap(i_param.getDistortionFactor(),scr_size);\r
// 解析オブジェクトを作る\r
- this._square_detect = new NyARSquareDetector_X2(i_param.getDistortionFactor(),scr_size);\r
- this._transmat = new NyARTransMat_X2(i_param);\r
+// this._square_detect = new NyARSquareDetector_X2(dist_factor_map,scr_size);\r
+// this._transmat = new NyARTransMat_X2(i_param,dist_factor_map);\r
+ this._square_detect = new NyARSquareDetector(i_param.getDistortionFactor(),scr_size);\r
+ this._transmat = new NyARTransMat_X2(i_param,dist_factor_map);\r
// 比較コードを保存\r
this._code = i_code;\r
this._marker_width = i_marker_width;\r
private NyARBinRaster _bin_raster;\r
private NyARRasterFilter_ARToolkitThreshold _tobin_filter=new NyARRasterFilter_ARToolkitThreshold(100);\r
\r
+ \r
/**\r
* i_imageにマーカー検出処理を実行し、結果を記録します。\r
* \r
throw new NyARException();\r
}\r
\r
- //ラスタを２値イメージに変換する.\r
+ //ラスタを(1/4の画像の)２値イメージに変換する.\r
this._tobin_filter.setThreshold(i_threshold);\r
this._tobin_filter.doFilter(i_raster,this._bin_raster);\r
\r
import jp.nyatla.nyartoolkit.core.types.*;\r
import jp.nyatla.nyartoolkit.core.param.*;\r
import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
-import jp.nyatla.nyartoolkit.core.pca2d.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARI64Linear;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARI64Point2d;\r
+import jp.nyatla.nyartoolkit.core2.types.matrix.NyARI64Matrix22;\r
import jp.nyatla.nyartoolkit.core.*;\r
\r
\r
*/\r
public class NyARSquareDetector_X2 implements INyARSquareDetector\r
{\r
- private static final double VERTEX_FACTOR = 1.0;// 線検出のファクタ\r
-\r
private static final int AR_AREA_MAX = 100000;// #define AR_AREA_MAX 100000\r
\r
private static final int AR_AREA_MIN = 70;// #define AR_AREA_MIN 70\r
private final NyARLabelingImage _limage;\r
\r
private final OverlapChecker _overlap_checker = new OverlapChecker();\r
- private final NyARCameraDistortionFactor _dist_factor_ref;\r
+ private final NyARFixedFloatCameraDistortionFactorMap _dist_factor_ref;\r
+// private final NyARFixFloatCameraDistortionFactorMap _dist_factor_ref;\r
+ private final NyARFixedFloatPca2d _pca;\r
+// private final INyARPca2d _pca;\r
\r
/**\r
* 最大i_squre_max個のマーカーを検出するクラスを作成する。\r
* \r
* @param i_param\r
*/\r
- public NyARSquareDetector_X2(NyARCameraDistortionFactor i_dist_factor_ref,NyARIntSize i_size) throws NyARException\r
+ public NyARSquareDetector_X2(NyARFixedFloatCameraDistortionFactorMap i_dist_factor_ref,NyARIntSize i_size) throws NyARException\r
{\r
this._width = i_size.w;\r
this._height = i_size.h;\r
this._xcoord = new int[number_of_coord * 2];\r
this._ycoord = new int[number_of_coord * 2];\r
//PCA\r
- this._pca=new NyARPca2d_SamplingPCA(20);\r
- \r
+\r
+ this._pca=new NyARFixedFloatPca2d();\r
}\r
+ private final int PCA_LENGTH=20;\r
\r
private final int _max_coord;\r
private final int[] _xcoord;\r
private final int[] _ycoord;\r
-\r
+ private final int[] _xpos=new int[PCA_LENGTH];\r
+ private final int[] _ypos=new int[PCA_LENGTH];\r
+ \r
private void normalizeCoord(int[] i_coord_x, int[] i_coord_y, int i_index, int i_coord_num)\r
{\r
// vertex1を境界にして、後方に配列を連結\r
return ret;\r
}\r
\r
- private final NyARVertexCounter __getSquareVertex_wv1 = new NyARVertexCounter();\r
+ private final NyARFixedFloatVertexCounter __getSquareVertex_wv1 = new NyARFixedFloatVertexCounter();\r
\r
- private final NyARVertexCounter __getSquareVertex_wv2 = new NyARVertexCounter();\r
+ private final NyARFixedFloatVertexCounter __getSquareVertex_wv2 = new NyARFixedFloatVertexCounter();\r
\r
/**\r
* static int arDetectMarker2_check_square( int area, ARMarkerInfo2 *marker_info2, double factor ) 関数の代替関数 OPTIMIZED STEP [450->415] o_squareに頂点情報をセットします。\r
*/\r
private boolean getSquareVertex(int[] i_x_coord, int[] i_y_coord, int i_vertex1_index, int i_coord_num, int i_area, int[] o_vertex)\r
{\r
- final NyARVertexCounter wv1 = this.__getSquareVertex_wv1;\r
- final NyARVertexCounter wv2 = this.__getSquareVertex_wv2;\r
+ final NyARFixedFloatVertexCounter wv1 = this.__getSquareVertex_wv1;\r
+ final NyARFixedFloatVertexCounter wv2 = this.__getSquareVertex_wv2;\r
final int end_of_coord = i_vertex1_index + i_coord_num - 1;\r
final int sx = i_x_coord[i_vertex1_index];// sx = marker_info2->x_coord[0];\r
final int sy = i_y_coord[i_vertex1_index];// sy = marker_info2->y_coord[0];\r
v1 = i;\r
}\r
}\r
- final double thresh = (i_area / 0.75) * 0.01 * VERTEX_FACTOR;\r
+ //final double thresh = (i_area / 0.75) * 0.01;\r
+ final long thresh_f16 =(i_area<<16)/75;\r
\r
o_vertex[0] = i_vertex1_index;\r
\r
- if (!wv1.getVertex(i_x_coord, i_y_coord, i_vertex1_index, v1, thresh)) { // if(get_vertex(marker_info2->x_coord,marker_info2->y_coord,0,v1,thresh,wv1,&wvnum1)<\r
+ if (!wv1.getVertex(i_x_coord, i_y_coord, i_vertex1_index, v1, thresh_f16)) { // if(get_vertex(marker_info2->x_coord,marker_info2->y_coord,0,v1,thresh,wv1,&wvnum1)<\r
// 0 ) {\r
return false;\r
}\r
- if (!wv2.getVertex(i_x_coord, i_y_coord, v1, end_of_coord, thresh)) {// if(get_vertex(marker_info2->x_coord,marker_info2->y_coord,v1,marker_info2->coord_num-1,thresh,wv2,&wvnum2)\r
+ if (!wv2.getVertex(i_x_coord, i_y_coord, v1, end_of_coord, thresh_f16)) {// if(get_vertex(marker_info2->x_coord,marker_info2->y_coord,v1,marker_info2->coord_num-1,thresh,wv2,&wvnum2)\r
// < 0) {\r
return false;\r
}\r
} else if (wv1.number_of_vertex > 1 && wv2.number_of_vertex == 0) {// }else if( wvnum1 > 1 && wvnum2== 0) {\r
//頂点位置を、起点から対角点の間の1/2にあると予想して、検索する。\r
v2 = (v1-i_vertex1_index)/2+i_vertex1_index;\r
- if (!wv1.getVertex(i_x_coord, i_y_coord, i_vertex1_index, v2, thresh)) {\r
+ if (!wv1.getVertex(i_x_coord, i_y_coord, i_vertex1_index, v2, thresh_f16)) {\r
return false;\r
}\r
- if (!wv2.getVertex(i_x_coord, i_y_coord, v2, v1, thresh)) {\r
+ if (!wv2.getVertex(i_x_coord, i_y_coord, v2, v1, thresh_f16)) {\r
return false;\r
}\r
if (wv1.number_of_vertex == 1 && wv2.number_of_vertex == 1) {\r
//v2 = (v1-i_vertex1_index+ end_of_coord-i_vertex1_index) / 2+i_vertex1_index;\r
v2 = (v1+ end_of_coord)/2;\r
\r
- if (!wv1.getVertex(i_x_coord, i_y_coord, v1, v2, thresh)) {\r
+ if (!wv1.getVertex(i_x_coord, i_y_coord, v1, v2, thresh_f16)) {\r
return false;\r
}\r
- if (!wv2.getVertex(i_x_coord, i_y_coord, v2, end_of_coord, thresh)) {\r
+ if (!wv2.getVertex(i_x_coord, i_y_coord, v2, end_of_coord, thresh_f16)) {\r
return false;\r
}\r
if (wv1.number_of_vertex == 1 && wv2.number_of_vertex == 1) {\r
return true;\r
}\r
\r
- private final INyARPca2d _pca;\r
- private final NyARDoubleMatrix22 __getSquareLine_evec=new NyARDoubleMatrix22();\r
- private final NyARDoublePoint2d __getSquareLine_mean=new NyARDoublePoint2d();\r
- private final NyARDoublePoint2d __getSquareLine_ev=new NyARDoublePoint2d();\r
+ private final NyARI64Matrix22 __getSquareLine_evec=new NyARI64Matrix22();\r
+ private final NyARI64Point2d __getSquareLine_mean=new NyARI64Point2d();\r
+ private final NyARI64Point2d __getSquareLine_ev=new NyARI64Point2d();\r
+ private final NyARI64Linear[] __getSquareLine_i64liner=NyARI64Linear.createArray(4);\r
+\r
/**\r
* 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\r
* coord_num,int vertex[], double line[4][3], double v[4][2], double *dist_factor) の２関数の合成品です。 マーカーのvertex,lineを計算して、結果をo_squareに保管します。\r
private boolean getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square) throws NyARException\r
{\r
final NyARLinear[] l_line = o_square.line;\r
- final NyARCameraDistortionFactor dist_factor=this._dist_factor_ref; \r
- final NyARDoubleMatrix22 evec=this.__getSquareLine_evec;\r
- final NyARDoublePoint2d mean=this.__getSquareLine_mean;\r
- final NyARDoublePoint2d ev=this.__getSquareLine_ev;\r
+ final NyARI64Matrix22 evec=this.__getSquareLine_evec;\r
+ final NyARI64Point2d mean=this.__getSquareLine_mean;\r
+ final NyARI64Point2d ev=this.__getSquareLine_ev;\r
+ final NyARI64Linear[] i64liner=this.__getSquareLine_i64liner;\r
\r
- \r
for (int i = 0; i < 4; i++) {\r
- final double w1 = (double) (i_mkvertex[i + 1] - i_mkvertex[i] + 1) * 0.05 + 0.5;\r
- final int st = (int) (i_mkvertex[i] + w1);\r
- final int ed = (int) (i_mkvertex[i + 1] - w1);\r
- final int n = ed - st + 1;\r
+// final double w1 = (double) (i_mkvertex[i + 1] - i_mkvertex[i] + 1) * 0.05 + 0.5;\r
+ final int w1 = ((((i_mkvertex[i + 1] - i_mkvertex[i] + 1)<<8)*13)>>8) + (1<<7);\r
+ final int st = i_mkvertex[i] + (w1>>8);\r
+ final int ed = i_mkvertex[i + 1] - (w1>>8);\r
+ int n = ed - st + 1;\r
if (n < 2) {\r
// nが2以下でmatrix.PCAを計算することはできないので、エラー\r
return false;\r
}\r
+ //配列作成\r
+ n=this._dist_factor_ref.observ2IdealSampling(i_xcoord, i_ycoord, st, n,this._xpos,this._ypos,PCA_LENGTH);\r
//主成分分析する。\r
- this._pca.pcaWithDistortionFactor(i_xcoord, i_ycoord, st, n,dist_factor, evec, ev,mean);\r
- final NyARLinear l_line_i = l_line[i];\r
+ this._pca.pcaF16(this._xpos,this._ypos, n,evec, ev,mean);\r
+ final NyARI64Linear l_line_i = i64liner[i];\r
l_line_i.run = evec.m01;// line[i][0] = evec->m[1];\r
l_line_i.rise = -evec.m00;// line[i][1] = -evec->m[0];\r
- l_line_i.intercept = -(l_line_i.run * mean.x + l_line_i.rise * mean.y);// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);\r
+ l_line_i.intercept = -((l_line_i.run * mean.x + l_line_i.rise * mean.y)>>16);// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);\r
}\r
\r
final NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex;\r
final NyARIntPoint[] l_imvertex = o_square.imvertex;\r
for (int i = 0; i < 4; i++) {\r
- final NyARLinear l_line_i = l_line[i];\r
- final NyARLinear l_line_2 = l_line[(i + 3) % 4];\r
- final double w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise;\r
- if (w1 == 0.0) {\r
+ final NyARI64Linear l_line_i = i64liner[i];\r
+ final NyARI64Linear l_line_2 = i64liner[(i + 3) % 4];\r
+ final long w1 =(l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise)>>16;\r
+ if (w1 == 0) {\r
return false;\r
}\r
- l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1;\r
- l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1;\r
+ l_sqvertex[i].x = (double)((l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1)/65536.0;\r
+ l_sqvertex[i].y = (double)((l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1)/65536.0;\r
// 頂点インデクスから頂点座標を得て保存\r
l_imvertex[i].x = i_xcoord[i_mkvertex[i]];\r
l_imvertex[i].y = i_ycoord[i_mkvertex[i]];\r
+ l_line[i].run=(double)l_line_i.run/65536.0;\r
+ l_line[i].rise=(double)l_line_i.rise/65536.0;\r
+ l_line[i].intercept=(double)l_line_i.intercept/65536.0;\r
}\r
return true;\r
}\r
}\r
+final class NyARFixedFloatVertexCounter\r
+{\r
+ public final int[] vertex = new int[6];// 5まで削れる\r
+\r
+ public int number_of_vertex;\r
+\r
+ private long thresh_16f;\r
+\r
+ private int[] x_coord;\r
+\r
+ private int[] y_coord;\r
+\r
+ public boolean getVertex(int[] i_x_coord, int[] i_y_coord, int st, int ed, long i_thresh)\r
+ {\r
+ this.number_of_vertex = 0;\r
+ this.thresh_16f = i_thresh;\r
+ this.x_coord = i_x_coord;\r
+ this.y_coord = i_y_coord;\r
+ return get_vertex(st, ed);\r
+ }\r
+\r
+ /**\r
+ * static int get_vertex( int x_coord[], int y_coord[], int st, int ed,double thresh, int vertex[], int *vnum) 関数の代替関数\r
+ * \r
+ * @param x_coord\r
+ * @param y_coord\r
+ * @param st\r
+ * @param ed\r
+ * @param thresh\r
+ * @return\r
+ */\r
+ private boolean get_vertex(int st, int ed)\r
+ {\r
+ int v1 = 0;\r
+ final int[] lx_coord = this.x_coord;\r
+ final int[] ly_coord = this.y_coord;\r
+ final int a = ly_coord[ed] - ly_coord[st];\r
+ final int b = lx_coord[st] - lx_coord[ed];\r
+ final int c = lx_coord[ed] * ly_coord[st] - ly_coord[ed] * lx_coord[st];\r
+ long dmax = 0;\r
+ for (int i = st + 1; i < ed; i++) {\r
+ final long d = a * lx_coord[i] + b * ly_coord[i] + c;\r
+ if (d * d > dmax) {\r
+ dmax = d * d;\r
+ v1 = i;\r
+ }\r
+ }\r
+ if ((dmax<<16) / (long)(a * a + b * b) > this.thresh_16f) {\r
+ if (!get_vertex(st, v1)) {\r
+ return false;\r
+ }\r
+ if (number_of_vertex > 5) {\r
+ return false;\r
+ }\r
+ vertex[number_of_vertex] = v1;// vertex[(*vnum)] = v1;\r
+ number_of_vertex++;// (*vnum)++;\r
\r
+ if (!get_vertex(v1, ed)) {\r
+ return false;\r
+ }\r
+ }\r
+ return true;\r
+ }\r
+}\r
\r
\r
/**\r
package jp.nyatla.nyartoolkit.sandbox.x2;\r
\r
import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.NyARSquare;\r
import jp.nyatla.nyartoolkit.core.param.*;\r
import jp.nyatla.nyartoolkit.core.transmat.*;\r
-import jp.nyatla.nyartoolkit.core.transmat.fitveccalc.NyARFitVecCalculator;\r
-import jp.nyatla.nyartoolkit.sandbox.x2.NyARSinTable;\r
-\r
+import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d;\r
+import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d;\r
\r
\r
/**\r
* 変換行列を計算して、結果を保持するクラス。\r
* \r
*/\r
-public class NyARTransMat_X2 extends NyARTransMat\r
+public class NyARTransMat_X2 implements INyARTransMat\r
{\r
- private NyARSinTable _sin_table=new NyARSinTable(1024);\r
- public NyARTransMat_X2(NyARParam i_param) throws NyARException\r
+ private final NyARFixedFloat16Point2d _center=new NyARFixedFloat16Point2d();\r
+ private final NyARFixedFloatTransOffset _offset=new NyARFixedFloatTransOffset();\r
+ private NyARFixedFloatRotMatrix _rotmatrix;\r
+ private NyARFixedFloatFitVecCalculator _calculator;\r
+ private NyARFixedFloatRotTransOptimize_O2 _mat_optimize;\r
+\r
+ public NyARTransMat_X2(NyARParam i_param,NyARFixedFloatCameraDistortionFactorMap i_dist_factor_map) throws NyARException\r
{\r
- super();\r
- final NyARCameraDistortionFactor dist=i_param.getDistortionFactor();\r
final NyARPerspectiveProjectionMatrix pmat=i_param.getPerspectiveProjectionMatrix();\r
- this._calculator=new NyARFitVecCalculator(pmat,dist);\r
- this._rotmatrix = new NyARRotMatrix_X2(pmat,this._sin_table);\r
- this._mat_optimize=new NyARRotTransOptimize_X2(pmat,this._sin_table);\r
+ this._calculator=new NyARFixedFloatFitVecCalculator(pmat,i_dist_factor_map);\r
+ this._rotmatrix = new NyARFixedFloatRotMatrix(pmat);\r
+ this._mat_optimize=new NyARFixedFloatRotTransOptimize_O2(pmat);\r
+ }\r
+\r
+ public void setCenter(double i_x, double i_y)\r
+ {\r
+ this._center.x= (long)i_x*NyMath.FIXEDFLOAT16_1;\r
+ this._center.y= (long)i_y*NyMath.FIXEDFLOAT16_1;\r
+ return;\r
+ }\r
+ /**\r
+ * 頂点順序をi_directionに対応して並べ替えます。\r
+ * @param i_square\r
+ * @param i_direction\r
+ * @param o_sqvertex_ref\r
+ * @param o_liner_ref\r
+ */\r
+ private final void initVertexOrder(NyARSquare i_square, int i_direction, NyARDoublePoint2d[] o_sqvertex_ref, NyARLinear[] o_liner_ref)\r
+ {\r
+ //頂点順序を考慮した矩形の頂点情報\r
+ o_sqvertex_ref[0]= i_square.sqvertex[(4 - i_direction) % 4];\r
+ o_sqvertex_ref[1]= i_square.sqvertex[(5 - i_direction) % 4];\r
+ o_sqvertex_ref[2]= i_square.sqvertex[(6 - i_direction) % 4];\r
+ o_sqvertex_ref[3]= i_square.sqvertex[(7 - i_direction) % 4]; \r
+ o_liner_ref[0]=i_square.line[(4 - i_direction) % 4];\r
+ o_liner_ref[1]=i_square.line[(5 - i_direction) % 4];\r
+ o_liner_ref[2]=i_square.line[(6 - i_direction) % 4];\r
+ o_liner_ref[3]=i_square.line[(7 - i_direction) % 4];\r
return;\r
}\r
+\r
+\r
+ private final NyARDoublePoint2d[] __transMat_sqvertex_ref = new NyARDoublePoint2d[4];\r
+ private final NyARLinear[] __transMat_linear_ref=new NyARLinear[4];\r
+ private final NyARFixedFloat16Point3d __transMat_trans=new NyARFixedFloat16Point3d();\r
+ /**\r
+ * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
+ * \r
+ * @param i_square\r
+ * 計算対象のNyARSquareオブジェクト\r
+ * @param i_direction\r
+ * @param i_width\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ public void transMat(final NyARSquare i_square, int i_direction, double i_width, NyARTransMatResult o_result_conv) throws NyARException\r
+ {\r
+ final NyARDoublePoint2d[] sqvertex_ref = __transMat_sqvertex_ref;\r
+ final NyARLinear[] linear_ref=__transMat_linear_ref;\r
+ final NyARFixedFloat16Point3d trans=this.__transMat_trans;\r
+ \r
+ //計算用に頂点情報を初期化（順番調整）\r
+ initVertexOrder(i_square, i_direction, sqvertex_ref,linear_ref);\r
+ \r
+ //基準矩形を設定\r
+ this._offset.setSquare((long)(i_width*NyMath.FIXEDFLOAT16_1),this._center);\r
+\r
+ // rotationを矩形情報から計算\r
+ this._rotmatrix.initRotBySquare(linear_ref,sqvertex_ref);\r
+\r
+ //平行移動量計算機にオフセット頂点をセット\r
+ this._calculator.setOffsetSquare(this._offset);\r
+ \r
+ //平行移動量計算機に適応先矩形の情報をセット\r
+ this._calculator.setFittedSquare(sqvertex_ref); \r
+\r
+ //回転行列の平行移動量の計算\r
+ this._calculator.calculateTransfer(this._rotmatrix,trans);\r
+ \r
+ //計算結果の最適化(this._rotmatrix,trans)\r
+ this._mat_optimize.optimize(this._rotmatrix,trans,this._calculator);\r
+ \r
+ // マトリクスの保存\r
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);\r
+ return;\r
+ }\r
+ /**\r
+ * double arGetTransMatCont( ARMarkerInfo *marker_info, double prev_conv[3][4],double center[2], double width, double conv[3][4] )\r
+ * \r
+ * @param i_square\r
+ * @param i_direction\r
+ * マーカーの方位を指定する。\r
+ * @param i_width\r
+ * @param io_result_conv\r
+ * 計算履歴を持つNyARTransMatResultオブジェクトを指定する。 履歴を持たない場合は、transMatと同じ処理を行う。\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ public void transMatContinue(NyARSquare i_square, int i_direction, double i_width, NyARTransMatResult io_result_conv) throws NyARException\r
+ {\r
+ /* 今度実装\r
+ final NyARDoublePoint2d[] sqvertex_ref = __transMat_sqvertex_ref;\r
+ final NyARLinear[] linear_ref=__transMat_linear_ref;\r
+ final NyARDoublePoint3d trans=this.__transMat_trans;\r
+\r
+ // io_result_convが初期値なら、transMatで計算する。\r
+ if (!io_result_conv.hasValue()) {\r
+ this.transMat(i_square, i_direction, i_width, io_result_conv);\r
+ return;\r
+ }\r
+ \r
+ //基準矩形を設定\r
+ this._offset.setSquare(i_width,this._center);\r
+\r
+ // rotationを矩形情報を一つ前の変換行列で初期化\r
+ this._rotmatrix.initRotByPrevResult(io_result_conv);\r
+\r
+ //平行移動量計算機に、オフセット頂点をセット\r
+ this._calculator.setOffsetSquare(this._offset);\r
+ \r
+ //平行移動量計算機に、適応先矩形の情報をセット\r
+ this._calculator.setFittedSquare(sqvertex_ref); \r
+ \r
+ //回転行列の平行移動量の計算\r
+ this._calculator.calculateTransfer(this._rotmatrix,trans);\r
+ \r
+ //計算結果の最適化(this._rotmatrix,trans)\r
+ final double err=this._mat_optimize.optimize(this._rotmatrix,trans,this._calculator);\r
+ \r
+ //計算結果を保存\r
+ io_result_conv.updateMatrixValue(this._rotmatrix, this._offset.point, trans);\r
+\r
+ // エラー値が許容範囲でなければTransMatをやり直し\r
+ if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) {\r
+ // rotationを矩形情報で初期化\r
+ this._rotmatrix.initRotBySquare(linear_ref,sqvertex_ref);\r
+ //回転行列の平行移動量の計算\r
+ this._calculator.calculateTransfer(this._rotmatrix,trans);\r
+ //計算結果の最適化(this._rotmatrix,trans)\r
+ final double err2=this._mat_optimize.optimize(this._rotmatrix,trans,this._calculator);\r
+ //エラー値が低かったら値を差換え\r
+ if (err2 < err) {\r
+ // 良い値が取れたら、差換え\r
+ io_result_conv.updateMatrixValue(this._rotmatrix, this._offset.point, trans);\r
+ }\r
+ } \r
+ return;*/\r
+ NyARException.notImplement();\r
+ }\r
+ public void updateMatrixValue(NyARFixedFloatRotMatrix i_rot, NyARFixedFloat16Point3d i_off, NyARFixedFloat16Point3d i_trans,NyARTransMatResult o_result)\r
+ {\r
+ o_result.m00=(double)i_rot.m00/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m01=(double)i_rot.m01/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m02=(double)i_rot.m02/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m03=(double)(((i_rot.m00 * i_off.x + i_rot.m01 * i_off.y + i_rot.m02 * i_off.z)>>24) + i_trans.x)/NyMath.FIXEDFLOAT16_1;\r
+\r
+ o_result.m10 =(double)i_rot.m10/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m11 =(double)i_rot.m11/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m12 =(double)i_rot.m12/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m13 =(double)(((i_rot.m10 * i_off.x + i_rot.m11 * i_off.y + i_rot.m12 * i_off.z)>>24) + i_trans.y)/NyMath.FIXEDFLOAT16_1;\r
+\r
+ o_result.m20 =(double)i_rot.m20/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m21 =(double)i_rot.m21/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m22 =(double)i_rot.m22/NyMath.FIXEDFLOAT24_1;\r
+ o_result.m23 =(double)(((i_rot.m20 * i_off.x + i_rot.m21 * i_off.y + i_rot.m22 * i_off.z)>>24) + i_trans.z)/NyMath.FIXEDFLOAT16_1;\r
+\r
+ o_result.has_value = true;\r
+ return;\r
+ } \r
}\r
--- /dev/null
+package jp.nyatla.nyartoolkit.sandbox.x2;\r
+\r
+public class NyMath\r
+{\r
+ public final static long FIXEDFLOAT24_1=0x1000000L;\r
+ public final static long FIXEDFLOAT16_1=0x10000L; \r
+ private final static int FF16_PI=(int)(Math.PI*FIXEDFLOAT16_1);\r
+ private final static int FF16_2PI=(int)(2 *FF16_PI);\r
+ private final static int SIN_RESOLUTION=1024;\r
+ private final static int ACOS_RESOLUTION=256;\r
+ /* sinテーブルは0-2PIを1024分割\r
+ * acosテーブルは0-1を256分割\r
+ */\r
+ private final static int[] sin_table=new int[SIN_RESOLUTION];\r
+ private final static int[] acos_table=new int[ACOS_RESOLUTION+1];\r
+ private final static int SQRT_LOOP=10;\r
+ /**\r
+ * http://www.geocities.co.jp/SiliconValley-PaloAlto/5438/\r
+ * 参考にしました。\r
+ * 少数点部が16bitの変数の平方根を求めます。\r
+ * 戻り値の小数点部分は16bitです。\r
+ * @param i_v\r
+ * @return\r
+ */\r
+ public static long sqrtFixdFloat16(long i_ff16)\r
+ {\r
+ long t=0,s;\r
+ s=i_ff16>0?i_ff16:-i_ff16;\r
+ if(i_ff16==0){\r
+ return 0;\r
+ }\r
+ for(int i=SQRT_LOOP;i>0;i--){\r
+ t = s;\r
+ s = (t+((i_ff16<<16)/t))>>1;\r
+ if(s==t){\r
+ break;\r
+ }\r
+ };\r
+ return t;\r
+ }\r
+ public static long sqrtFixdFloat(long i_ff,int i_bit)\r
+ {\r
+ long t=0,s;\r
+ s=i_ff>0?i_ff:-i_ff;\r
+ if(i_ff==0){\r
+ return 0;\r
+ }\r
+ for(int i=SQRT_LOOP;i>0;i--){\r
+ t = s;\r
+ s = (t+((i_ff<<i_bit)/t))>>1;\r
+ if(s==t){\r
+ break;\r
+ }\r
+ }\r
+ return t;\r
+ }\r
+ public static int acosFixedFloat16(int i_ff24)\r
+ {\r
+ // 0～1を0～256に変換\r
+ int idx = (int)((i_ff24)>>16);//S8\r
+ if (idx < 0) {\r
+ return acos_table[-idx]+FF16_PI;\r
+ }else{\r
+ return acos_table[idx];\r
+ }\r
+ } \r
+ public static int sinFixedFloat24(int i_ff16)\r
+ {\r
+ // 0～2PIを0～1024に変換\r
+ int rad_index = (int) (i_ff16 * SIN_RESOLUTION / FF16_2PI);\r
+ rad_index = rad_index % SIN_RESOLUTION;\r
+ if (rad_index < 0) {\r
+ rad_index += SIN_RESOLUTION;\r
+ }\r
+ // ここで0-1024にいる\r
+ return sin_table[rad_index];\r
+ }\r
+ public static int cosFixedFloat24(int i_ff16)\r
+ {\r
+ // 0～Math.PI/2を 0～256の値空間に変換\r
+ int rad_index = (int) (i_ff16* SIN_RESOLUTION / FF16_2PI);\r
+ // 90度ずらす\r
+ rad_index = (rad_index + SIN_RESOLUTION / 4) % SIN_RESOLUTION;\r
+ // 負の領域に居たら、+1024しておく\r
+ if (rad_index < 0) {\r
+ rad_index += SIN_RESOLUTION;\r
+ }\r
+ // ここで0-1024にいる\r
+ return sin_table[rad_index]; \r
+// return (int)(Math.cos((double)i_ff16/0x10000)*0x1000000);\r
+ }\r
+ public static void initialize()\r
+ {\r
+ //解像度は4の倍数で無いとダメ\r
+ assert(SIN_RESOLUTION%4==0);\r
+\r
+ int d4 = SIN_RESOLUTION / 4;\r
+ //sinテーブル初期化(0-2PIを0-1024に作成)\r
+ for (int i = 1; i < SIN_RESOLUTION; i++) {\r
+ sin_table[i] =(int)((Math.sin(2 * Math.PI * (double) i / (double) SIN_RESOLUTION))*FIXEDFLOAT24_1);\r
+ }\r
+ sin_table[0] = 0;\r
+ sin_table[d4 - 1] = 0x1000000;\r
+ sin_table[d4 * 2 - 1] = 0;\r
+ sin_table[d4 * 3 - 1] = -0x1000000;\r
+ //acosテーブル初期化\r
+ for (int i = 1; i < ACOS_RESOLUTION; i++) {\r
+ acos_table[i] =(int)((Math.acos((double) i / (double) ACOS_RESOLUTION))*FIXEDFLOAT16_1);\r
+ }\r
+ acos_table[0]=FF16_PI;\r
+ acos_table[ACOS_RESOLUTION]=0;\r
+ return;\r
+ }\r
+ public static void printF16(long i_value)\r
+ {\r
+ System.out.println((double)i_value/0x10000);\r
+ return;\r
+ }\r
+ public static void printF24(long i_value)\r
+ {\r
+ System.out.println((double)i_value/0x1000000);\r
+ return;\r
+ }\r
+}\r
\r
public RawFileTest_X2()\r
{\r
+ NyMath.initialize();\r
}\r
\r
\r
--- /dev/null
+/* このソースは実験用のソースです。
+ * 動いたり動かなかったりします。
+ *
+ */
+package jp.nyatla.nyartoolkit.sandbox.x2;
+
+import javax.media.*;
+
+import javax.media.util.BufferToImage;
+import javax.media.format.*;
+
+import jp.nyatla.nyartoolkit.NyARException;
+import jp.nyatla.nyartoolkit.jmf.utils.*;
+import jp.nyatla.nyartoolkit.sandbox.quadx2.NyARRasterFilter_ARTTh_Quad;
+
+import jp.nyatla.nyartoolkit.core.*;
+
+import java.awt.*;
+
+import jp.nyatla.nyartoolkit.core.labeling.*;
+import jp.nyatla.nyartoolkit.core.param.*;
+import jp.nyatla.nyartoolkit.core.raster.*;
+import jp.nyatla.nyartoolkit.core.rasterfilter.*;
+import jp.nyatla.nyartoolkit.core2.rasterfilter.rgb2gs.*;
+import jp.nyatla.nyartoolkit.core2.rasterfilter.gs2bin.*;
+import jp.nyatla.utils.j2se.LabelingBufferdImage;
+import jp.nyatla.nyartoolkit.core2.rasteranalyzer.threshold.*;
+
+
+
+public class VisualTest extends Frame implements JmfCaptureListener
+{
+ private final String camera_file = "../../Data/camera_para.dat";
+
+ private JmfNyARRaster_RGB _raster;
+
+ private JmfCameraCapture capture;
+ private NyARParam ap;
+ public VisualTest() throws NyARException, NyARException
+ {
+ setBounds(0, 0, 640 + 64, 720 + 64);
+ // キャプチャの準備
+ capture = new JmfCameraCapture(320, 240, 30f, JmfCameraCapture.PIXEL_FORMAT_RGB);
+ capture.setCaptureListener(this);
+
+ // キャプチャイメージ用のラスタを準備
+ this._raster = new JmfNyARRaster_RGB(320, 240);
+
+ // AR用カメラパラメタファイルをロード
+ ap = new NyARParam();
+ ap.loadARParamFromFile(camera_file);
+ ap.changeScreenSize(320, 240);
+
+
+ }
+
+ private NyARBinRaster _binraster1 = new NyARBinRaster(160,120);
+
+ private NyARGrayscaleRaster _gsraster1 = new NyARGrayscaleRaster(320, 240);
+ private INyARRasterThresholdAnalyzer _tha=new NyARRasterThresholdAnalyzer_DiffHistgram();
+
+ private LabelingBufferdImage _bimg = new LabelingBufferdImage(320, 240);
+ private LabelingBufferdImage _bimg2 = new LabelingBufferdImage(160, 120);
+ private NyARRasterFilter_ARTTh_Quad _tobin_filter=new NyARRasterFilter_ARTTh_Quad(100);
+
+ public void onUpdateBuffer(Buffer i_buffer)
+ {
+
+ try {
+ // キャプチャしたバッファをラスタにセット
+ _raster.setBuffer(i_buffer);
+
+ Graphics g = getGraphics();
+ // キャプチャ画像
+ BufferToImage b2i = new BufferToImage((VideoFormat) i_buffer.getFormat());
+ Image img = b2i.createImage(i_buffer);
+ this.getGraphics().drawImage(img, 32, 32, this);
+
+ // 画像1
+ INyARRasterFilter_RgbToGs filter_rgb2gs = new NyARRasterFilter_RgbAve();
+// INyARRasterFilter_RgbToGs filter_rgb2gs = new NyARRasterFilter_RgbMul();
+
+ filter_rgb2gs.doFilter(_raster, _gsraster1);
+ this._bimg.drawImage(this._gsraster1);
+ this.getGraphics().drawImage(this._bimg, 32 + 320, 32, 320 + 320 + 32, 240 + 32, 0, 240, 320, 0, this);
+ _tha.analyzeRaster(_gsraster1);
+ NyARRasterFilter_Threshold gs2bin=new NyARRasterFilter_Threshold(_tha.getThreshold());
+
+
+ // 画像2
+ _tobin_filter.doFilter(_raster, _binraster1);
+ this._bimg2.drawImage(_binraster1);
+ this.getGraphics().drawImage(this._bimg2, 32, 32 + 240, 320 + 32, 240 + 32 + 240, 0, 240, 320, 0, this);
+
+ // 画像3
+ NyARLabelingImage limage = new NyARLabelingImage(320, 240);
+ NyARLabeling_ARToolKit labeling = new NyARLabeling_ARToolKit();
+ labeling.attachDestination(limage);
+ labeling.labeling(_binraster1);
+ this._bimg.drawImage(this._gsraster1);
+
+ NyARSquareStack stack = new NyARSquareStack(100);
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
+
+ }
+
+ private void startCapture()
+ {
+ try {
+ capture.start();
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
+ }
+
+ public static void main(String[] args)
+ {
+ try {
+ VisualTest mainwin = new VisualTest();
+ mainwin.setVisible(true);
+ mainwin.startCapture();
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
+
+ }
+
+}
*/\r
protected double[][] m;\r
\r
- private int clm, row;\r
+ protected int clm, row;\r
\r
/**\r
* デフォルトコンストラクタは機能しません。\r
public class NyARSquareDetector implements INyARSquareDetector\r
{\r
private static final double VERTEX_FACTOR = 1.0;// 線検出のファクタ\r
-\r
private static final int AR_AREA_MAX = 100000;// #define AR_AREA_MAX 100000\r
-\r
private static final int AR_AREA_MIN = 70;// #define AR_AREA_MIN 70\r
private final int _width;\r
private final int _height;\r
private final NyARLabelingImage _limage;\r
\r
private final OverlapChecker _overlap_checker = new OverlapChecker();\r
- private final NyARCameraDistortionFactor _dist_factor_ref;\r
+ private final INyARCameraDistortionFactor _dist_factor;\r
+ \r
+ private final double[] _xpos;\r
+ private final double[] _ypos;\r
\r
/**\r
* 最大i_squre_max個のマーカーを検出するクラスを作成する。\r
{\r
this._width = i_size.w;\r
this._height = i_size.h;\r
- this._dist_factor_ref = i_dist_factor_ref;\r
+ //歪み計算テーブルを作ると、8*width/height*2の領域を消費します。\r
+ //領域を取りたくない場合は、i_dist_factor_refの値をそのまま使ってください。\r
+ this._dist_factor = new NyARCameraDistortionFactorMap(i_dist_factor_ref,i_size);\r
this._labeling = new NyARLabeling_ARToolKit();\r
this._limage = new NyARLabelingImage(this._width, this._height);\r
this._labeling.attachDestination(this._limage);\r
this._max_coord = number_of_coord;\r
this._xcoord = new int[number_of_coord * 2];\r
this._ycoord = new int[number_of_coord * 2];\r
- //PCA(最大頂点数は対角線長さ)\r
- this._pca=new NyARPca2d_MatrixPCA_O2(this._width+this._height);\r
- \r
+ this._pca=new NyARPca2d_MatrixPCA_O2();\r
+ this._xpos=new double[this._width+this._height];//最大辺長はthis._width+this._height\r
+ this._ypos=new double[this._width+this._height];//最大辺長はthis._width+this._height\r
+ return;\r
}\r
\r
private final int _max_coord;\r
private boolean getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square) throws NyARException\r
{\r
final NyARLinear[] l_line = o_square.line;\r
- final NyARCameraDistortionFactor dist_factor=this._dist_factor_ref; \r
final NyARDoubleMatrix22 evec=this.__getSquareLine_evec;\r
final NyARDoublePoint2d mean=this.__getSquareLine_mean;\r
final NyARDoublePoint2d ev=this.__getSquareLine_ev;\r
// nが2以下でmatrix.PCAを計算することはできないので、エラー\r
return false;\r
}\r
+ //配列作成\r
+ this._dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st, n,this._xpos,this._ypos);\r
+ \r
//主成分分析する。\r
- this._pca.pcaWithDistortionFactor(i_xcoord, i_ycoord, st, n,dist_factor, evec, ev,mean);\r
+ this._pca.pca(this._xpos,this._ypos,n,evec, ev,mean);\r
final NyARLinear l_line_i = l_line[i];\r
l_line_i.run = evec.m01;// line[i][0] = evec->m[1];\r
l_line_i.rise = -evec.m00;// line[i][1] = -evec->m[0];\r
*/\r
final public class NyARVertexCounter\r
{\r
- public final int[] vertex = new int[10];// 5まで削れる\r
+ public final int[] vertex = new int[10];// 6まで削れる\r
\r
public int number_of_vertex;\r
\r
*/\r
private boolean get_vertex(int st, int ed)\r
{\r
+ //メモ:座標値は65536を超えなければint32で扱って大丈夫なので変更。\r
+ //dmaxは4乗なのでやるとしてもint64じゃないとマズイ\r
int v1 = 0;\r
final int[] lx_coord = this.x_coord;\r
final int[] ly_coord = this.y_coord;\r
- final double a = ly_coord[ed] - ly_coord[st];\r
- final double b = lx_coord[st] - lx_coord[ed];\r
- final double c = lx_coord[ed] * ly_coord[st] - ly_coord[ed] * lx_coord[st];\r
+ final int a = ly_coord[ed] - ly_coord[st];\r
+ final int b = lx_coord[st] - lx_coord[ed];\r
+ final int c = lx_coord[ed] * ly_coord[st] - ly_coord[ed] * lx_coord[st];\r
double dmax = 0;\r
for (int i = st + 1; i < ed; i++) {\r
final double d = a * lx_coord[i] + b * ly_coord[i] + c;\r
v1 = i;\r
}\r
}\r
- if (dmax / (a * a + b * b) > thresh) {\r
+ if (dmax / (double)(a * a + b * b) > thresh) {\r
if (!get_vertex(st, v1)) {\r
return false;\r
}\r
int sx=getTopClipTangentX(label);\r
int sy=label.clip_t;\r
\r
- int coord_num = 1;// marker_info2->coord_num = 1;\r
- o_coord_x[0] = sx;// marker_info2->x_coord[0] = sx;\r
- o_coord_y[0] = sy;// marker_info2->y_coord[0] = sy;\r
+ int coord_num = 1;\r
+ o_coord_x[0] = sx;\r
+ o_coord_y[0] = sy;\r
int dir = 5;\r
\r
int[][] limage=this._ref_buf;\r
for (;;) {\r
dir = (dir + 5) % 8;\r
for (i = 0; i < 8; i++) {\r
- if (limage[r + ydir[dir]][c + xdir[dir]] > 0) {// if(\r
- // p1[ydir[dir]*xsize+xdir[dir]] > 0 ){\r
+ if (limage[r + ydir[dir]][c + xdir[dir]] > 0) {\r
break;\r
}\r
dir = (dir + 1) % 8;\r
throw new NyARException();// return(-1);\r
}\r
// xcoordとycoordをc,rにも保存\r
- c = c + xdir[dir];// marker_info2->x_coord[marker_info2->coord_num]=marker_info2->x_coord[marker_info2->coord_num-1]\r
- // + xdir[dir];\r
- r = r + ydir[dir];// marker_info2->y_coord[marker_info2->coord_num]=marker_info2->y_coord[marker_info2->coord_num-1]+ ydir[dir];\r
- o_coord_x[coord_num] = c;// marker_info2->x_coord[marker_info2->coord_num]=marker_info2->x_coord[marker_info2->coord_num-1]+ xdir[dir];\r
- o_coord_y[coord_num] = r;// marker_info2->y_coord[marker_info2->coord_num]=marker_info2->y_coord[marker_info2->coord_num-1]+ ydir[dir];\r
+ c = c + xdir[dir];\r
+ r = r + ydir[dir];\r
+ o_coord_x[coord_num] = c;\r
+ o_coord_y[coord_num] = r;\r
// 終了条件判定\r
if (c == sx && r == sy){\r
coord_num++;\r
break;\r
}\r
coord_num++;\r
- if (coord_num == i_array_size) {// if( marker_info2.coord_num ==Config.AR_CHAIN_MAX-1 ){\r
+ if (coord_num == i_array_size) {\r
//輪郭が末端に達した\r
return coord_num;\r
}\r
--- /dev/null
+package jp.nyatla.nyartoolkit.core.param;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
+\r
+public interface INyARCameraDistortionFactor\r
+{\r
+ /**\r
+ * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数\r
+ * \r
+ * @param i_in\r
+ * @param o_out\r
+ */\r
+ public void ideal2Observ(final NyARDoublePoint2d i_in, NyARDoublePoint2d o_out);\r
+ /**\r
+ * ideal2Observをまとめて実行します。\r
+ * @param i_in\r
+ * @param o_out\r
+ */\r
+ public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size);\r
+ /**\r
+ * int arParamObserv2Ideal( const double dist_factor[4], const double ox,const double oy,double *ix, double *iy );\r
+ * \r
+ * @param ix\r
+ * @param iy\r
+ * @param ix\r
+ * @param iy\r
+ * @return\r
+ */\r
+ public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point);\r
+ /**\r
+ * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。\r
+ * @param i_x_coord\r
+ * @param i_y_coord\r
+ * @param i_start\r
+ * coord開始点\r
+ * @param i_num\r
+ * 計算数\r
+ * @param o_ideal\r
+ * 出力バッファ[i_num][2]であること。\r
+ */\r
+ public void observ2IdealBatch(int[] i_x_coord, int[] i_y_coord,int i_start, int i_num, double[] o_x_coord,double[] o_y_coord);\r
+\r
+}\r
* p=(1-fd^2)\r
* xd=px+x0,yd=py+y0\r
*/\r
-final public class NyARCameraDistortionFactor\r
+public class NyARCameraDistortionFactor implements INyARCameraDistortionFactor\r
{\r
private static final int PD_LOOP = 3;\r
private double _f0;//x0\r
private double _f1;//y0\r
private double _f2;//100000000.0*ｆ\r
private double _f3;//s\r
+ public void copyFrom(NyARCameraDistortionFactor i_ref)\r
+ {\r
+ this._f0=i_ref._f0;\r
+ this._f1=i_ref._f1;\r
+ this._f2=i_ref._f2;\r
+ this._f3=i_ref._f3;\r
+ return;\r
+ }\r
/**\r
* 配列の値をファクタ値としてセットする。\r
* @param i_factor\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core.param;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+/**\r
+ * 歪み成分マップを使用するINyARCameraDistortionFactor\r
+ */\r
+final public class NyARCameraDistortionFactorMap implements INyARCameraDistortionFactor\r
+{\r
+ private double[] _factor=new double[4];\r
+ private int _stride;\r
+ private double[] _mapx;\r
+ private double[] _mapy;\r
+ /**\r
+ * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数\r
+ * \r
+ * @param i_in\r
+ * @param o_out\r
+ */\r
+ public void ideal2Observ(final NyARDoublePoint2d i_in, NyARDoublePoint2d o_out)\r
+ {\r
+ final double f0=this._factor[0];\r
+ final double f1=this._factor[1];\r
+ final double x = (i_in.x - f0) * this._factor[3];\r
+ final double y = (i_in.y - f1) * this._factor[3];\r
+ if (x == 0.0 && y == 0.0) {\r
+ o_out.x = f0;\r
+ o_out.y = f1;\r
+ } else {\r
+ final double d = 1.0 - this._factor[2] / 100000000.0 * (x * x + y * y);\r
+ o_out.x = x * d + f0;\r
+ o_out.y = y * d + f1;\r
+ }\r
+ return;\r
+ }\r
+\r
+ /**\r
+ * ideal2Observをまとめて実行します。\r
+ * @param i_in\r
+ * @param o_out\r
+ */\r
+ public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)\r
+ {\r
+ double x, y;\r
+ final double d0 = this._factor[0];\r
+ final double d1 = this._factor[1];\r
+ final double d3 = this._factor[3];\r
+ final double d2_w = this._factor[2] / 100000000.0;\r
+ for (int i = 0; i < i_size; i++) {\r
+ x = (i_in[i].x - d0) * d3;\r
+ y = (i_in[i].y - d1) * d3;\r
+ if (x == 0.0 && y == 0.0) {\r
+ o_out[i].x = d0;\r
+ o_out[i].y = d1;\r
+ } else {\r
+ final double d = 1.0 - d2_w * (x * x + y * y);\r
+ o_out[i].x = x * d + d0;\r
+ o_out[i].y = y * d + d1;\r
+ }\r
+ }\r
+ return;\r
+ } \r
+ \r
+ \r
+ \r
+ public NyARCameraDistortionFactorMap(NyARCameraDistortionFactor i_distfactor,NyARIntSize i_screen_size)\r
+ {\r
+ NyARDoublePoint2d opoint=new NyARDoublePoint2d();\r
+ this._mapx=new double[i_screen_size.w*i_screen_size.h];\r
+ this._mapy=new double[i_screen_size.w*i_screen_size.h];\r
+ this._stride=i_screen_size.w;\r
+ int ptr=i_screen_size.h*i_screen_size.w-1;\r
+ //歪みマップを構築\r
+ for(int i=i_screen_size.h-1;i>=0;i--)\r
+ {\r
+ for(int i2=i_screen_size.w-1;i2>=0;i2--)\r
+ {\r
+ i_distfactor.observ2Ideal(i2,i, opoint);\r
+ this._mapx[ptr]=opoint.x;\r
+ this._mapy[ptr]=opoint.y;\r
+ ptr--;\r
+ }\r
+ }\r
+ i_distfactor.getValue(this._factor);\r
+ return;\r
+ }\r
+ public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)\r
+ {\r
+ int idx=(int)ix+(int)iy*this._stride;\r
+ o_point.x=this._mapx[idx];\r
+ o_point.y=this._mapy[idx];\r
+ return;\r
+ }\r
+ public void observ2IdealBatch(int[] i_x_coord, int[] i_y_coord,int i_start, int i_num, double[] o_x_coord,double[] o_y_coord)\r
+ {\r
+ int idx;\r
+ int ptr=0;\r
+ for (int j = 0; j < i_num; j++) {\r
+ idx=i_x_coord[i_start + j]+i_y_coord[i_start + j]*this._stride;\r
+ o_x_coord[ptr]=this._mapx[idx];\r
+ o_y_coord[ptr]=this._mapy[idx];\r
+ ptr++;\r
+ }\r
+ return;\r
+ } \r
+}\r
* @param o_mean\r
* @throws NyARException\r
*/\r
- public void pca(double[] i_x,double[] i_y,int i_start,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException;\r
+ public void pca(double[] i_x,double[] i_y,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException;\r
/**\r
* カメラ歪み補正つきのPCA\r
* @param i_x\r
* @param o_mean\r
* @throws NyARException\r
*/\r
- public void pcaWithDistortionFactor(int[] i_x,int[] i_y,int i_start,int i_number_of_point,NyARCameraDistortionFactor i_factor,NyARDoubleMatrix22 o_evec,NyARDoublePoint2d o_ev, NyARDoublePoint2d o_mean) throws NyARException;\r
+// public void pcaWithDistortionFactor(int[] i_x,int[] i_y,int i_start,int i_number_of_point,INyARCameraDistortionFactor i_factor,NyARDoubleMatrix22 o_evec,NyARDoublePoint2d o_ev, NyARDoublePoint2d o_mean) throws NyARException;\r
}\r
import jp.nyatla.nyartoolkit.NyARException;\r
import jp.nyatla.nyartoolkit.core.NyARMat;\r
import jp.nyatla.nyartoolkit.core.NyARVec;\r
-import jp.nyatla.nyartoolkit.core.param.NyARCameraDistortionFactor;\r
import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
import jp.nyatla.nyartoolkit.core.types.matrix.NyARDoubleMatrix22;\r
/**\r
private final NyARVec __pca_ev = new NyARVec(2);\r
private final NyARVec __pca_mean = new NyARVec(2); \r
\r
- public void pca(double[] i_x,double[] i_y,int i_start,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
- {\r
- final NyARMat input = this.__pca_input;// 次処理で初期化される。\r
- final double[][] input_array=input.getArray();\r
- // pcaの準備\r
- input.realloc(i_number_of_point, 2);\r
- System.arraycopy(i_x, 0, input_array[0], i_start, i_number_of_point);\r
- System.arraycopy(i_y, 0, input_array[1], i_start, i_number_of_point);\r
- // 主成分分析\r
- input.matrixPCA(this.__pca_evec, this.__pca_ev, this.__pca_mean);\r
- final double[] mean_array = this.__pca_mean.getArray();\r
- final double[][] evec_array = this.__pca_evec.getArray();\r
- final double[] ev_array=this.__pca_ev.getArray();\r
- o_evec.m00=evec_array[0][0];\r
- o_evec.m01=evec_array[0][1];\r
- o_evec.m10=evec_array[1][0];\r
- o_evec.m11=evec_array[1][1];\r
- o_ev.x=ev_array[0];\r
- o_ev.x=ev_array[1];\r
- o_mean.x=mean_array[0];\r
- o_mean.y=mean_array[1];\r
- return;\r
- \r
- }\r
-\r
- public void pcaWithDistortionFactor(int[] i_x,int[] i_y,int i_start,int i_number_of_point,NyARCameraDistortionFactor i_factor,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
+ public void pca(double[] i_x,double[] i_y,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
{\r
final NyARMat input = this.__pca_input;// 次処理で初期化される。 \r
// pcaの準備\r
input.realloc(i_number_of_point, 2);\r
final double[][] input_array=input.getArray();\r
- i_factor.observ2IdealBatch(i_x, i_y, i_start, i_number_of_point,input_array[0],input_array[1]);\r
+ for(int i=0;i<i_number_of_point;i++){\r
+ input_array[i][0]=i_x[i];\r
+ input_array[i][1]=i_y[i];\r
+ }\r
// 主成分分析\r
input.matrixPCA(this.__pca_evec, this.__pca_ev, this.__pca_mean);\r
final double[] mean_array = this.__pca_mean.getArray();\r
package jp.nyatla.nyartoolkit.core.pca2d;\r
\r
import jp.nyatla.nyartoolkit.NyARException;\r
-import jp.nyatla.nyartoolkit.core.param.NyARCameraDistortionFactor;\r
import jp.nyatla.nyartoolkit.core.types.*;\r
import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
\r
*/\r
public class NyARPca2d_MatrixPCA_O2 implements INyARPca2d\r
{\r
- private double[] _x;\r
-\r
- private double[] _y;\r
-\r
- private int _number_of_data;\r
-\r
private static final double PCA_EPS = 1e-6; // #define EPS 1e-6\r
\r
private static final int PCA_MAX_ITER = 100; // #define MAX_ITER 100\r
\r
private static final double PCA_VZERO = 1e-16; // #define VZERO 1e-16\r
- public NyARPca2d_MatrixPCA_O2(int i_max_points)\r
- {\r
- this._x=new double[i_max_points];\r
- this._y=new double[i_max_points];\r
- this._number_of_data=0;\r
- return;\r
- }\r
-\r
\r
/**\r
* static int QRM( ARMat *a, ARVec *dv )の代替関数\r
* @param o_ev\r
* @throws NyARException\r
*/\r
- private void PCA_PCA(NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
+ private void PCA_PCA(double[] i_x,double[] i_y,int i_number_of_data,NyARDoubleMatrix22 o_matrix, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
{\r
// double[] mean_array=mean.getArray();\r
// mean.zeroClear();\r
//PCA_EXの処理\r
double sx = 0;\r
double sy = 0;\r
- final int number_of_data = this._number_of_data;\r
- for (int i = 0; i < number_of_data; i++) {\r
- sx += this._x[i];\r
- sy += this._y[i];\r
+ for (int i = 0; i < i_number_of_data; i++) {\r
+ sx += i_x[i];\r
+ sy += i_y[i];\r
}\r
- sx = sx / number_of_data;\r
- sy = sy / number_of_data;\r
+ sx = sx / i_number_of_data;\r
+ sy = sy / i_number_of_data;\r
\r
//PCA_CENTERとPCA_xt_by_xを一緒に処理\r
- final double srow = Math.sqrt((double) this._number_of_data);\r
+ final double srow = Math.sqrt((double) i_number_of_data);\r
double w00, w11, w10;\r
w00 = w11 = w10 = 0.0;// *out = 0.0;\r
- for (int i = 0; i < number_of_data; i++) {\r
- final double x = (this._x[i] - sx) / srow;\r
- final double y = (this._y[i] - sy) / srow;\r
+ for (int i = 0; i < i_number_of_data; i++) {\r
+ final double x = (i_x[i] - sx) / srow;\r
+ final double y = (i_y[i] - sy) / srow;\r
w00 += (x * x);// *out += *in1 * *in2;\r
w10 += (x * y);// *out += *in1 * *in2;\r
w11 += (y * y);// *out += *in1 * *in2;\r
// }\r
return;\r
}\r
- public void pca(double[] i_x,double[] i_y,int i_start,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
+ public void pca(double[] i_x,double[] i_y,int i_number_of_point,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
{\r
- NyARException.trap("未チェックの関数");\r
- assert(this._x.length>i_number_of_point); \r
- this._number_of_data = i_number_of_point;\r
- System.arraycopy(i_x, 0, this._x, i_start, i_number_of_point);\r
- System.arraycopy(i_y, 0, this._y, i_start, i_number_of_point);\r
- \r
- PCA_PCA(o_evec, o_ev,o_mean);\r
+ PCA_PCA(i_x,i_y,i_number_of_point,o_evec, o_ev,o_mean);\r
\r
final double sum = o_ev.x + o_ev.y;\r
// For順変更禁止\r
o_ev.y /= sum;// ev->v[i] /= sum;\r
return; \r
}\r
- \r
- public void pcaWithDistortionFactor(int[] i_x,int[] i_y,int i_start,int i_number_of_point,NyARCameraDistortionFactor i_factor,NyARDoubleMatrix22 o_evec, NyARDoublePoint2d o_ev,NyARDoublePoint2d o_mean) throws NyARException\r
- {\r
- assert(this._x.length>i_number_of_point);\r
- this._number_of_data = i_number_of_point;\r
- i_factor.observ2IdealBatch(i_x, i_y, i_start, i_number_of_point, this._x, this._y);\r
-\r
- PCA_PCA(o_evec,o_ev,o_mean);\r
\r
- final double sum = o_ev.x + o_ev.y;\r
- // For順変更禁止\r
- o_ev.x /= sum;// ev->v[i] /= sum;\r
- o_ev.y /= sum;// ev->v[i] /= sum;\r
- return; \r
- }\r
}\r
\r
public interface INyARBufferReader\r
{\r
+ // ID規約\r
+ // 00-07(8)型番号\r
+ // 08-15(8)ビットフォーマットID\r
+ // 00:24bit/01:32bit/02:16bit\r
+ // 16-27(8)型ID\r
+ // 00:無効/01:byte[]/02:int[][]/03:short[]\r
+ // 24-31(8)予約\r
+ //\r
+ \r
/**\r
* RGB24フォーマットで、全ての画素が0\r
*/\r
public static final int BUFFERFORMAT_BYTE1D_B8G8R8X8_32 = 0x00010101;\r
\r
/**\r
- * byte[]で、RGB565の16ビット(little endian)で画素が格納されている。\r
+ * byte[]で、RGB565の16ビット(little/big endian)で画素が格納されている。\r
*/\r
public static final int BUFFERFORMAT_BYTE1D_R5G6B5_16LE = 0x00010201;\r
+ public static final int BUFFERFORMAT_BYTE1D_R5G6B5_16BE = 0x00010202;\r
+ /**\r
+ * short[]で、RGB565の16ビット(little/big endian)で画素が格納されている。\r
+ */ \r
+ public static final int BUFFERFORMAT_WORD1D_R5G6B5_16LE = 0x00030201;\r
+ public static final int BUFFERFORMAT_WORD1D_R5G6B5_16BE = 0x00030202;\r
+\r
\r
/**\r
* int[][]で特に値範囲を定めない\r
final NyARPerspectiveProjectionMatrix pmat=i_param.getPerspectiveProjectionMatrix();\r
this._calculator=new NyARFitVecCalculator(pmat,dist);\r
this._rotmatrix = new NyARRotMatrix_ARToolKit(pmat);\r
- this._mat_optimize=new NyARRotTransOptimize(pmat);\r
+ this._mat_optimize=new NyARRotTransOptimize_O2(pmat);\r
}\r
\r
public void setCenter(double i_x, double i_y)\r
this._mat_optimize.optimize(this._rotmatrix,trans,this._calculator);\r
\r
// マトリクスの保存\r
- o_result_conv.updateMatrixValue(this._rotmatrix, this._offset.point, trans);\r
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);\r
return;\r
}\r
/**\r
final NyARDoublePoint3d trans=this.__transMat_trans;\r
\r
// io_result_convが初期値なら、transMatで計算する。\r
- if (!io_result_conv.hasValue()) {\r
+ if (!io_result_conv.has_value) {\r
this.transMat(i_square, i_direction, i_width, io_result_conv);\r
return;\r
}\r
final double err=this._mat_optimize.optimize(this._rotmatrix,trans,this._calculator);\r
\r
//計算結果を保存\r
- io_result_conv.updateMatrixValue(this._rotmatrix, this._offset.point, trans);\r
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,io_result_conv);\r
\r
// エラー値が許容範囲でなければTransMatをやり直し\r
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) {\r
//エラー値が低かったら値を差換え\r
if (err2 < err) {\r
// 良い値が取れたら、差換え\r
- io_result_conv.updateMatrixValue(this._rotmatrix, this._offset.point, trans);\r
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,io_result_conv);\r
}\r
}\r
return;\r
}\r
+ /**\r
+ * パラメータで変換行列を更新します。\r
+ * \r
+ * @param i_rot\r
+ * @param i_off\r
+ * @param i_trans\r
+ */\r
+ public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_off, NyARDoublePoint3d i_trans,NyARTransMatResult o_result)\r
+ {\r
+ o_result.m00=i_rot.m00;\r
+ o_result.m01=i_rot.m01;\r
+ o_result.m02=i_rot.m02;\r
+ o_result.m03=i_rot.m00 * i_off.x + i_rot.m01 * i_off.y + i_rot.m02 * i_off.z + i_trans.x;\r
+\r
+ o_result.m10 = i_rot.m10;\r
+ o_result.m11 = i_rot.m11;\r
+ o_result.m12 = i_rot.m12;\r
+ o_result.m13 = i_rot.m10 * i_off.x + i_rot.m11 * i_off.y + i_rot.m12 * i_off.z + i_trans.y;\r
+\r
+ o_result.m20 = i_rot.m20;\r
+ o_result.m21 = i_rot.m21;\r
+ o_result.m22 = i_rot.m22;\r
+ o_result.m23 = i_rot.m20 * i_off.x + i_rot.m21 * i_off.y + i_rot.m22 * i_off.z + i_trans.z;\r
+\r
+ o_result.has_value = true;\r
+ return;\r
+ } \r
}\r
package jp.nyatla.nyartoolkit.core.transmat;\r
\r
\r
-import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix;\r
-import jp.nyatla.nyartoolkit.core.types.*;\r
import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
\r
/**\r
*/\r
public class NyARTransMatResult extends NyARDoubleMatrix34\r
{\r
- private boolean has_value = false;\r
-\r
-\r
- /**\r
- * パラメータで変換行列を更新します。\r
- * \r
- * @param i_rot\r
- * @param i_off\r
- * @param i_trans\r
- */\r
- public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_off, NyARDoublePoint3d i_trans)\r
- {\r
- this.m00=i_rot.m00;\r
- this.m01=i_rot.m01;\r
- this.m02=i_rot.m02;\r
- this.m03=i_rot.m00 * i_off.x + i_rot.m01 * i_off.y + i_rot.m02 * i_off.z + i_trans.x;\r
-\r
- this.m10 = i_rot.m10;\r
- this.m11 = i_rot.m11;\r
- this.m12 = i_rot.m12;\r
- this.m13 = i_rot.m10 * i_off.x + i_rot.m11 * i_off.y + i_rot.m12 * i_off.z + i_trans.y;\r
-\r
- this.m20 = i_rot.m20;\r
- this.m21 = i_rot.m21;\r
- this.m22 = i_rot.m22;\r
- this.m23 = i_rot.m20 * i_off.x + i_rot.m21 * i_off.y + i_rot.m22 * i_off.z + i_trans.z;\r
-\r
- this.has_value = true;\r
- return;\r
- }\r
-\r
- public boolean hasValue()\r
- {\r
- return this.has_value;\r
- }\r
+ public boolean has_value = false;\r
}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core.transmat.optimize;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core.transmat.fitveccalc.NyARFitVecCalculator;\r
+import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+\r
+/**\r
+ * 処理構造がわかる程度に展開したNyARRotTransOptimize\r
+ * \r
+ */\r
+public class NyARRotTransOptimize_Base implements INyARRotTransOptimize\r
+{\r
+ private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
+\r
+ private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
+\r
+ private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
+\r
+ public NyARRotTransOptimize_Base(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
+ {\r
+ this._projection_mat_ref = i_projection_mat_ref;\r
+ return;\r
+ }\r
+\r
+ final public double optimize(NyARRotMatrix io_rotmat, NyARDoublePoint3d io_transvec, NyARFitVecCalculator i_calculator) throws NyARException\r
+ {\r
+ final NyARDoublePoint2d[] fit_vertex = i_calculator.getFitSquare();\r
+ final NyARDoublePoint3d[] offset_square = i_calculator.getOffsetVertex().vertex;\r
+\r
+ double err = -1;\r
+ /* ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。 */\r
+ for (int i = 0;; i++) {\r
+ // <arGetTransMat3>\r
+ err = modifyMatrix(io_rotmat, io_transvec, offset_square, fit_vertex);\r
+ i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
+ err = modifyMatrix(io_rotmat, io_transvec, offset_square, fit_vertex);\r
+ // //</arGetTransMat3>\r
+ if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR || i == AR_GET_TRANS_MAT_MAX_LOOP_COUNT - 1) {\r
+ break;\r
+ }\r
+ i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
+ }\r
+ return err;\r
+ }\r
+ private double[] __createRotationMap_b_map=new double[6];\r
+ private double[] __createRotationMap_c_map=new double[6];\r
+ private double[] __createRotationMap_f=new double[3];\r
+ private void createRotationMap(NyARDoublePoint3d i_angle,double i_factor,NyARDoubleMatrix33[] i_rot_matrix)\r
+ {\r
+ double sina,cosa,sinb,cosb,sinc,cosc;\r
+ double CACA,SASA,SACA,SASB,CASB,SACACB,CACACB,SASACB;\r
+\r
+ \r
+ final double[] f=this.__createRotationMap_f;\r
+ final double[] b_map=this.__createRotationMap_b_map;\r
+ final double[] c_map=this.__createRotationMap_c_map;\r
+ f[0]=-i_factor;\r
+ f[1]=0;\r
+ f[2]=i_factor;\r
+ double ang1,ang2;\r
+ //BとCのsinマップを先に作成\r
+ for(int i=0;i<3;i++)\r
+ {\r
+ ang1=i_angle.y + f[i];\r
+ b_map[i] =Math.sin(ang1);\r
+ b_map[i+3]=Math.cos(ang1);\r
+ ang2=i_angle.z + f[i];\r
+ c_map[i] =Math.sin(ang2);\r
+ c_map[i+3]=Math.cos(ang2);\r
+ }\r
+ int idx=0;\r
+ int t1,t2,t3;\r
+ for (t1 = 0; t1 < 3; t1++){\r
+ ang1=i_angle.x + f[t1];\r
+ sina = Math.sin(ang1);\r
+ cosa = Math.cos(ang1); \r
+ CACA = cosa * cosa;\r
+ SASA = sina * sina;\r
+ SACA = sina * cosa;\r
+\r
+ for (t2=0;t2<3;t2++){\r
+ sinb = b_map[t2];\r
+ cosb = b_map[t2+3];\r
+ SASB = sina * sinb;\r
+ CASB = cosa * sinb;\r
+ SACACB = SACA * cosb;\r
+ CACACB = CACA * cosb;\r
+ SASACB = SASA * cosb; \r
+ for (t3=0;t3<3;t3++) {\r
+ sinc = c_map[t3];\r
+ cosc = c_map[t3+3];\r
+ final NyARDoubleMatrix33 mat_ptr=i_rot_matrix[idx];\r
+ mat_ptr.m00 = CACACB * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
+ mat_ptr.m01 = -CACACB * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
+ mat_ptr.m02 = CASB;\r
+ mat_ptr.m10 = SACACB * cosc - SACA * cosc + SASACB * sinc + CACA * sinc;\r
+ mat_ptr.m11 = -SACACB * sinc + SACA * sinc + SASACB * cosc + CACA * cosc;\r
+ mat_ptr.m12 = SASB;\r
+ mat_ptr.m20 = -CASB * cosc - SASB * sinc;\r
+ mat_ptr.m21 = CASB * sinc - SASB * cosc;\r
+ mat_ptr.m22 = cosb;\r
+ idx++;\r
+ }\r
+ }\r
+ }\r
+ return;\r
+ }\r
+ private final void getNewMatrix(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, NyARDoubleMatrix34 o_combo)\r
+ {\r
+ double cp0,cp1,cp2,cp3;\r
+ NyARPerspectiveProjectionMatrix cp=this._projection_mat_ref;\r
+\r
+ cp3=cp.m03;\r
+ cp0=cp.m00;cp1=cp.m01;cp2=cp.m02;\r
+ o_combo.m00=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
+ o_combo.m01=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
+ o_combo.m02=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
+ o_combo.m03=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
+\r
+ cp0=cp.m10;cp1=cp.m11;cp2=cp.m12;\r
+ o_combo.m10=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
+ o_combo.m11=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
+ o_combo.m12=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
+ o_combo.m13=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
+\r
+ cp0=cp.m20;cp1=cp.m21;cp2=cp.m22;\r
+ o_combo.m20=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
+ o_combo.m21=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
+ o_combo.m22=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
+ o_combo.m23=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
+ return;\r
+ }\r
+ private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d();\r
+ private final NyARDoubleMatrix34 __modifyMatrix_combo=new NyARDoubleMatrix34();\r
+ private final NyARDoubleMatrix33[] __modifyMatrix_next_rot_matrix=NyARDoubleMatrix33.createArray(27); \r
+ public final double modifyMatrix(NyARRotMatrix io_rot, NyARDoublePoint3d trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) throws NyARException\r
+ {\r
+ final NyARDoublePoint3d angle = this.__modifyMatrix_angle;\r
+ final NyARDoubleMatrix34 combo=this.__modifyMatrix_combo;\r
+ final NyARDoubleMatrix33[] next_rot_matrix=this.__modifyMatrix_next_rot_matrix;\r
+ double factor;\r
+ double hx, hy, h, x, y;\r
+ double err, minerr = 0;\r
+ int i,i2;\r
+ int best_idx=0;\r
+\r
+ io_rot.getAngle(angle);// arGetAngle( rot, &a, &b, &c );\r
+ factor = 10.0 * Math.PI / 180.0;\r
+ for (int j = 0; j < 10; j++){\r
+ minerr = 1000000000.0;\r
+ //評価用の角度マップ作成\r
+ createRotationMap(angle,factor,next_rot_matrix);\r
+ //評価して一番宜しいIDを保存\r
+ best_idx=(1+1*3+1*9);\r
+ for(i2=0;i2<27;i2++){\r
+ this.getNewMatrix(next_rot_matrix[i2],trans,combo);\r
+ err = 0.0;\r
+ for (i = 0; i < 4; i++) {\r
+ hx = combo.m00 * i_vertex3d[i].x + combo.m01 * i_vertex3d[i].y + combo.m02 * i_vertex3d[i].z + combo.m03;\r
+ hy = combo.m10 * i_vertex3d[i].x + combo.m11 *i_vertex3d[i].y + combo.m12 * i_vertex3d[i].z + combo.m13;\r
+ h = combo.m20 * i_vertex3d[i].x + combo.m21 * i_vertex3d[i].y + combo.m22 * i_vertex3d[i].z + combo.m23;\r
+ x = i_vertex2d[i].x-(hx / h);\r
+ y = i_vertex2d[i].y-(hy / h);\r
+ err += x*x+y*y;\r
+ }\r
+ if (err < minerr){\r
+ minerr = err;\r
+ best_idx=i2;\r
+ }\r
+ \r
+ }\r
+ if (best_idx==(1+1*3+1*9)){\r
+ factor *= 0.5; \r
+ }else{\r
+ angle.z+=factor*(best_idx%3-1);\r
+ angle.y+=factor*((best_idx/3)%3-1);\r
+ angle.x+=factor*((best_idx/9)%3-1); \r
+ }\r
+ }\r
+ io_rot.setAngle(angle.x,angle.y,angle.z);\r
+ /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
+ return minerr / 4;\r
+ }\r
+\r
+\r
+\r
+\r
+}\r
* <airmail(at)ebony.plala.or.jp>\r
* \r
*/\r
-package jp.nyatla.nyartoolkit.sandbox.x2;\r
+package jp.nyatla.nyartoolkit.core.transmat.optimize;\r
\r
\r
import jp.nyatla.nyartoolkit.NyARException;\r
import jp.nyatla.nyartoolkit.core.param.*;\r
-import jp.nyatla.nyartoolkit.core.transmat.fitveccalc.*;\r
-import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*;\r
-import jp.nyatla.nyartoolkit.core.types.*;\r
-import jp.nyatla.nyartoolkit.core.transmat.optimize.*;\r
-import jp.nyatla.nyartoolkit.sandbox.x2.NyARSinTable;\r
-\r
+import jp.nyatla.nyartoolkit.core.transmat.fitveccalc.NyARFitVecCalculator;\r
+import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix;\r
+import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
+import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint3d;\r
/**\r
* 基本姿勢と実画像を一致するように、角度を微調整→平行移動量を再計算\r
* を繰り返して、変換行列を最適化する。\r
*\r
*/\r
-public class NyARRotTransOptimize_X2 implements INyARRotTransOptimize\r
+public class NyARRotTransOptimize_O2 implements INyARRotTransOptimize\r
{\r
- private NyARSinTable _sin_table_ref; \r
private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
- public NyARRotTransOptimize_X2(NyARPerspectiveProjectionMatrix i_projection_mat_ref,NyARSinTable _sin_table_ref)\r
+ public NyARRotTransOptimize_O2(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
{\r
- this._sin_table_ref=_sin_table_ref;\r
this._projection_mat_ref=i_projection_mat_ref;\r
return;\r
}\r
{\r
final NyARDoublePoint2d[] fit_vertex=i_calculator.getFitSquare();\r
final NyARDoublePoint3d[] offset_square=i_calculator.getOffsetVertex().vertex;\r
-\r
\r
double err = -1;\r
/*ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。*/\r
{\r
double factor;\r
double a2, b2, c2;\r
- double ma = 0.0, mb = 0.0, mc = 0.0;\r
double h, x, y;\r
double err, minerr = 0;\r
int t1, t2, t3;\r
- int s1 = 0, s2 = 0, s3 = 0;\r
+ int best_idx=0;\r
\r
factor = 10.0 * Math.PI / 180.0;\r
- double rot0, rot1, rot3, rot4, rot6, rot7;\r
+ double rot0, rot1, rot2;\r
double combo00, combo01, combo02, combo03, combo10, combo11, combo12, combo13, combo20, combo21, combo22, combo23;\r
double combo02_2, combo02_5, combo02_8, combo02_11;\r
double combo22_2, combo22_5, combo22_8, combo22_11;\r
double combo12_2, combo12_5, combo12_8, combo12_11;\r
// vertex展開\r
final double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32;\r
- NyARDoublePoint3d d_pt;\r
- d_pt = i_vertex3d[0];\r
- VX00 = d_pt.x;\r
- VX01 = d_pt.y;\r
- VX02 = d_pt.z;\r
- d_pt = i_vertex3d[1];\r
- VX10 = d_pt.x;\r
- VX11 = d_pt.y;\r
- VX12 = d_pt.z;\r
- d_pt = i_vertex3d[2];\r
- VX20 = d_pt.x;\r
- VX21 = d_pt.y;\r
- VX22 = d_pt.z;\r
- d_pt = i_vertex3d[3];\r
- VX30 = d_pt.x;\r
- VX31 = d_pt.y;\r
- VX32 = d_pt.z;\r
+ VX00 = i_vertex3d[0].x;\r
+ VX01 = i_vertex3d[0].y;\r
+ VX02 = i_vertex3d[0].z;\r
+ VX10 = i_vertex3d[1].x;\r
+ VX11 = i_vertex3d[1].y;\r
+ VX12 = i_vertex3d[1].z;\r
+ VX20 = i_vertex3d[2].x;\r
+ VX21 = i_vertex3d[2].y;\r
+ VX22 = i_vertex3d[2].z;\r
+ VX30 = i_vertex3d[3].x;\r
+ VX31 = i_vertex3d[3].y;\r
+ VX32 = i_vertex3d[3].z;\r
final double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31;\r
- NyARDoublePoint2d d_pt2;\r
- d_pt2 = i_vertex2d[0];\r
- P2D00 = d_pt2.x;\r
- P2D01 = d_pt2.y;\r
- d_pt2 = i_vertex2d[1];\r
- P2D10 = d_pt2.x;\r
- P2D11 = d_pt2.y;\r
- d_pt2 = i_vertex2d[2];\r
- P2D20 = d_pt2.x;\r
- P2D21 = d_pt2.y;\r
- d_pt2 = i_vertex2d[3];\r
- P2D30 = d_pt2.x;\r
- P2D31 = d_pt2.y;\r
+ P2D00 = i_vertex2d[0].x;\r
+ P2D01 = i_vertex2d[0].y;\r
+ P2D10 = i_vertex2d[1].x;\r
+ P2D11 = i_vertex2d[1].y;\r
+ P2D20 = i_vertex2d[2].x;\r
+ P2D21 = i_vertex2d[2].y;\r
+ P2D30 = i_vertex2d[3].x;\r
+ P2D31 = i_vertex2d[3].y;\r
final NyARPerspectiveProjectionMatrix prjmat = this._projection_mat_ref;\r
- final double CP0, CP1, CP2, CP4, CP5, CP6, CP8, CP9, CP10;\r
- CP0 = prjmat.m00;\r
- CP1 = prjmat.m01;\r
- CP2 = prjmat.m02;\r
- CP4 = prjmat.m10;\r
- CP5 = prjmat.m11;\r
- CP6 = prjmat.m12;\r
- CP8 = prjmat.m20;\r
- CP9 = prjmat.m21;\r
- CP10 = prjmat.m22;\r
+ final double CP0 = prjmat.m00,CP1 = prjmat.m01,CP2 = prjmat.m02,CP4 = prjmat.m10,CP5 = prjmat.m11,CP6 = prjmat.m12,CP8 = prjmat.m20,CP9 = prjmat.m21,CP10 = prjmat.m22;\r
combo03 = CP0 * trans.x + CP1 * trans.y + CP2 * trans.z + prjmat.m03;\r
combo13 = CP4 * trans.x + CP5 * trans.y + CP6 * trans.z + prjmat.m13;\r
combo23 = CP8 * trans.x + CP9 * trans.y + CP10 * trans.z + prjmat.m23;\r
final double[][] double1D = this.__modifyMatrix_double1D;\r
\r
final NyARDoublePoint3d angle = this.__modifyMatrix_angle;\r
-\r
final double[] a_factor = double1D[1];\r
final double[] sinb = double1D[2];\r
final double[] cosb = double1D[3];\r
a_factor[j] = w;\r
w = b2 + w2;\r
b_factor[j] = w;\r
- sinb[j] = this._sin_table_ref.sin(w);\r
- cosb[j] = this._sin_table_ref.cos(w);\r
+ sinb[j] = Math.sin(w);\r
+ cosb[j] = Math.cos(w);\r
w = c2 + w2;\r
c_factor[j] = w;\r
- sinc[j] = this._sin_table_ref.sin(w);\r
- cosc[j] = this._sin_table_ref.cos(w);\r
+ sinc[j] = Math.sin(w);\r
+ cosc[j] = Math.cos(w);\r
}\r
//\r
for (t1 = 0; t1 < 3; t1++) {\r
- SA = this._sin_table_ref.sin(a_factor[t1]);\r
- CA = this._sin_table_ref.cos(a_factor[t1]);\r
+ SA = Math.sin(a_factor[t1]);\r
+ CA = Math.cos(a_factor[t1]);\r
// Optimize\r
CACA = CA * CA;\r
SASA = SA * SA;\r
SACACBCC = SACACB * wcos;\r
\r
rot0 = CACACB * wcos + SASA * wcos + SACACBSC - SACASC;\r
- rot3 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin;\r
- rot6 = -CASB * wcos - SASB * wsin;\r
-\r
- combo00 = CP0 * rot0 + CP1 * rot3 + CP2 * rot6;\r
- combo10 = CP4 * rot0 + CP5 * rot3 + CP6 * rot6;\r
- combo20 = CP8 * rot0 + CP9 * rot3 + CP10 * rot6;\r
+ rot1 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin;\r
+ rot2 = -CASB * wcos - SASB * wsin;\r
+ combo00 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2;\r
+ combo10 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2;\r
+ combo20 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2;\r
\r
- rot1 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC;\r
- rot4 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos;\r
- rot7 = CASB * wsin - SASB * wcos;\r
- combo01 = CP0 * rot1 + CP1 * rot4 + CP2 * rot7;\r
- combo11 = CP4 * rot1 + CP5 * rot4 + CP6 * rot7;\r
- combo21 = CP8 * rot1 + CP9 * rot4 + CP10 * rot7;\r
+ rot0 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC;\r
+ rot1 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos;\r
+ rot2 = CASB * wsin - SASB * wcos;\r
+ combo01 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2;\r
+ combo11 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2;\r
+ combo21 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2;\r
//\r
err = 0.0;\r
h = combo20 * VX00 + combo21 * VX01 + combo22_2;\r
err += x * x + y * y;\r
if (err < minerr) {\r
minerr = err;\r
- ma = a_factor[t1];\r
- mb = b_factor[t2];\r
- mc = c_factor[t3];\r
- s1 = t1 - 1;\r
- s2 = t2 - 1;\r
- s3 = t3 - 1;\r
+ a2 = a_factor[t1];\r
+ b2 = b_factor[t2];\r
+ c2 = c_factor[t3];\r
+ best_idx=t1+t2*3+t3*9;\r
}\r
}\r
}\r
}\r
- if (s1 == 0 && s2 == 0 && s3 == 0) {\r
+ if (best_idx==(1+3+9)) {\r
factor *= 0.5;\r
}\r
- a2 = ma;\r
- b2 = mb;\r
- c2 = mc;\r
}\r
- io_rot.setAngle(ma, mb, mc);\r
+ io_rot.setAngle(a2, b2, c2);\r
/* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
- return minerr / 4;\r
- }\r
+ return minerr /4;\r
+ } \r
+ \r
+ \r
}\r
*/\r
package jp.nyatla.nyartoolkit.core.types.matrix;\r
\r
+\r
public class NyARDoubleMatrix33 implements INyARDoubleMatrix\r
{\r
public double m00;\r
public double m20;\r
public double m21;\r
public double m22;\r
+ public static NyARDoubleMatrix33[] createArray(int i_number)\r
+ {\r
+ NyARDoubleMatrix33[] ret=new NyARDoubleMatrix33[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARDoubleMatrix33();\r
+ }\r
+ return ret;\r
+ }\r
/**\r
* 遅いからあんまり使わないでね。\r
*/\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types;\r
+\r
+public class NyARFixedFloat16Point2d extends NyARI64Point2d\r
+{\r
+ public static NyARFixedFloat16Point2d[] createArray(int i_number)\r
+ {\r
+ NyARFixedFloat16Point2d[] ret=new NyARFixedFloat16Point2d[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARFixedFloat16Point2d();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types;\r
+\r
+public class NyARFixedFloat16Point3d extends NyARI64Point3d\r
+{\r
+ /**\r
+ * 配列ファクトリ\r
+ * @param i_number\r
+ * @return\r
+ */\r
+ public static NyARFixedFloat16Point3d[] createArray(int i_number)\r
+ {\r
+ NyARFixedFloat16Point3d[] ret=new NyARFixedFloat16Point3d[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARFixedFloat16Point3d();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types;\r
+\r
+public class NyARI64Linear\r
+{\r
+ public long rise;//y軸の増加量\r
+ public long run;//x軸の増加量\r
+ public long intercept;//切片\r
+ public final void copyFrom(NyARI64Linear i_source)\r
+ {\r
+ this.rise=i_source.rise;\r
+ this.run=i_source.run;\r
+ this.intercept=i_source.intercept;\r
+ return;\r
+ }\r
+ public static NyARI64Linear[] createArray(int i_number)\r
+ {\r
+ NyARI64Linear[] ret=new NyARI64Linear[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARI64Linear();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types;\r
+\r
+public class NyARI64Point2d\r
+{\r
+ public long x;\r
+\r
+ public long y;\r
+ /**\r
+ * 配列ファクトリ\r
+ * @param i_number\r
+ * @return\r
+ */\r
+ public static NyARI64Point2d[] createArray(int i_number)\r
+ {\r
+ NyARI64Point2d[] ret=new NyARI64Point2d[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARI64Point2d();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types;\r
+\r
+public class NyARI64Point3d\r
+{\r
+ public long x;\r
+ public long y;\r
+ public long z;\r
+ /**\r
+ * 配列ファクトリ\r
+ * @param i_number\r
+ * @return\r
+ */\r
+ public static NyARI64Point3d[] createArray(int i_number)\r
+ {\r
+ NyARI64Point3d[] ret=new NyARI64Point3d[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARI64Point3d();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types.matrix;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.matrix.NyARDoubleMatrix33;\r
+\r
+\r
+public class NyARFixedFloat16Matrix33 extends NyARI64Matrix33\r
+{\r
+ public void copyFrom(NyARDoubleMatrix33 i_matrix)\r
+ {\r
+ this.m00=(long)i_matrix.m00*0x10000;\r
+ this.m01=(long)i_matrix.m01*0x10000;\r
+ this.m02=(long)i_matrix.m02*0x10000;\r
+ this.m10=(long)i_matrix.m10*0x10000;\r
+ this.m11=(long)i_matrix.m11*0x10000;\r
+ this.m12=(long)i_matrix.m12*0x10000;\r
+ this.m20=(long)i_matrix.m20*0x10000;\r
+ this.m21=(long)i_matrix.m21*0x10000;\r
+ this.m22=(long)i_matrix.m22*0x10000;\r
+ return;\r
+ }\r
+ public static NyARFixedFloat16Matrix33[] createArray(int i_number)\r
+ {\r
+ NyARFixedFloat16Matrix33[] ret=new NyARFixedFloat16Matrix33[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARFixedFloat16Matrix33();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types.matrix;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.matrix.NyARDoubleMatrix33;\r
+\r
+\r
+public class NyARFixedFloat24Matrix33 extends NyARI64Matrix33\r
+{\r
+ public void copyFrom(NyARDoubleMatrix33 i_matrix)\r
+ {\r
+ this.m00=(long)i_matrix.m00*0x1000000;\r
+ this.m01=(long)i_matrix.m01*0x1000000;\r
+ this.m02=(long)i_matrix.m02*0x1000000;\r
+ this.m10=(long)i_matrix.m10*0x1000000;\r
+ this.m11=(long)i_matrix.m11*0x1000000;\r
+ this.m12=(long)i_matrix.m12*0x1000000;\r
+ this.m20=(long)i_matrix.m20*0x1000000;\r
+ this.m21=(long)i_matrix.m21*0x1000000;\r
+ this.m22=(long)i_matrix.m22*0x1000000;\r
+ return;\r
+ }\r
+ public static NyARFixedFloat24Matrix33[] createArray(int i_number)\r
+ {\r
+ NyARFixedFloat24Matrix33[] ret=new NyARFixedFloat24Matrix33[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARFixedFloat24Matrix33();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /dev/null
+package jp.nyatla.nyartoolkit.core2.types.matrix;\r
+\r
+public class NyARI64Matrix22\r
+{\r
+ public long m00;\r
+ public long m01;\r
+ public long m10;\r
+ public long m11;\r
+}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core2.types.matrix;\r
+\r
+\r
+public class NyARI64Matrix33\r
+{\r
+ public long m00;\r
+ public long m01;\r
+ public long m02;\r
+ public long m10;\r
+ public long m11;\r
+ public long m12;\r
+ public long m20;\r
+ public long m21;\r
+ public long m22;\r
+ public static NyARI64Matrix33[] createArray(int i_number)\r
+ {\r
+ NyARI64Matrix33[] ret=new NyARI64Matrix33[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARI64Matrix33();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
\r
// マーカーを検出\r
Date d2 = new Date();\r
- for (int i = 0; i < 1000; i++) {\r
+ for (int i = 0; i < 3000; i++) {\r
// 変換行列を取得\r
ar.detectMarkerLite(ra, 100);\r
ar.getTransmationMatrix(result_mat);\r
OSDN Git Service
