From: shinobu_siv Date: Wed, 21 May 2008 13:31:57 +0000 (+0000) Subject: 初期インポート。 X-Git-Url: http://git.osdn.net/view?a=commitdiff_plain;h=988cc14cf184d3f789a03f47b94d78a2c66d98dd;p=nyartoolkit-and%2Fnyartoolkit-android-0.9.git 初期インポート。 --- diff --git a/.classpath b/.classpath new file mode 100644 index 0000000..e92639a --- /dev/null +++ b/.classpath @@ -0,0 +1,6 @@ + + + + + + diff --git a/.project b/.project new file mode 100644 index 0000000..4ad716e --- /dev/null +++ b/.project @@ -0,0 +1,33 @@ + + + NyARToolkitAndroid + + + + + + com.android.ide.eclipse.adt.ResourceManagerBuilder + + + + + com.android.ide.eclipse.adt.PreCompilerBuilder + + + + + org.eclipse.jdt.core.javabuilder + + + + + com.android.ide.eclipse.adt.ApkBuilder + + + + + + com.android.ide.eclipse.adt.AndroidNature + org.eclipse.jdt.core.javanature + + diff --git a/.settings/org.eclipse.core.resources.prefs b/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..5555380 --- /dev/null +++ b/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Thu Apr 17 18:22:05 JST 2008 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/AndroidManifest.xml b/AndroidManifest.xml new file mode 100644 index 0000000..ee9e367 --- /dev/null +++ b/AndroidManifest.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/bin/NyARToolkitAndroid.apk b/bin/NyARToolkitAndroid.apk new file mode 100644 index 0000000..e65759f Binary files /dev/null and b/bin/NyARToolkitAndroid.apk differ diff --git a/bin/classes.dex b/bin/classes.dex new file mode 100644 index 0000000..79bcede Binary files /dev/null and b/bin/classes.dex differ diff --git a/bin/resources.ap_ b/bin/resources.ap_ new file mode 100644 index 0000000..53fe3e5 Binary files /dev/null and b/bin/resources.ap_ differ diff --git a/res/drawable/icon.png b/res/drawable/icon.png new file mode 100644 index 0000000..64e3601 Binary files /dev/null and b/res/drawable/icon.png differ diff --git a/res/layout/main.xml b/res/layout/main.xml new file mode 100644 index 0000000..f1069fa --- /dev/null +++ b/res/layout/main.xml @@ -0,0 +1,15 @@ + + + + + + + diff --git a/res/raw/camera_para.dat b/res/raw/camera_para.dat new file mode 100644 index 0000000..eb671b1 Binary files /dev/null and b/res/raw/camera_para.dat differ diff --git a/res/raw/marker.jpg b/res/raw/marker.jpg new file mode 100644 index 0000000..75a84f3 Binary files /dev/null and b/res/raw/marker.jpg differ diff --git a/res/raw/patt.hiro b/res/raw/patt.hiro new file mode 100644 index 0000000..7f78e3c --- /dev/null +++ b/res/raw/patt.hiro @@ -0,0 +1,196 @@ + 234 235 240 233 240 234 240 235 240 237 240 238 240 240 240 232 + 229 240 240 240 240 240 240 240 240 240 240 240 240 240 240 228 + 227 240 240 240 240 240 240 240 240 240 240 240 240 240 240 239 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 236 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 234 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 236 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 231 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 229 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 225 149 240 240 186 216 225 174 240 240 240 237 238 240 240 240 + 150 107 238 231 75 208 115 147 238 228 223 226 237 180 226 240 + 150 62 181 213 62 187 113 169 197 72 29 237 120 50 53 207 + 149 63 47 78 53 184 113 101 142 5 150 150 45 217 186 83 + 121 84 220 222 58 180 121 92 128 109 237 124 155 232 161 64 + 149 71 240 240 76 210 98 109 122 108 240 129 51 119 161 155 + 149 186 240 240 98 219 135 152 207 191 236 227 152 77 175 209 + 235 235 240 233 240 234 240 235 240 236 240 238 240 240 240 240 + 229 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 227 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 236 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 234 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 236 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 232 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 229 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 225 156 240 240 186 216 225 186 240 240 240 240 240 240 240 240 + 150 117 240 231 72 206 115 162 240 232 223 237 240 180 226 240 + 150 74 187 213 51 184 103 168 197 78 29 237 120 50 53 216 + 144 77 51 74 61 184 106 101 142 5 150 152 52 217 186 85 + 117 89 219 219 65 184 121 92 128 100 236 125 156 240 170 73 + 148 71 240 240 76 210 109 109 121 99 240 137 51 120 166 164 + 140 186 240 240 98 220 150 156 207 192 236 230 152 77 176 212 + 234 235 240 233 240 234 240 235 240 236 240 238 240 240 240 233 + 229 240 240 240 240 240 240 240 240 240 240 240 240 240 240 239 + 227 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 234 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 232 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 235 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 232 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 228 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 225 156 240 240 182 212 225 180 240 240 240 240 240 240 240 240 + 150 116 238 228 66 205 115 151 238 236 225 240 240 180 226 240 + 156 84 186 211 47 184 109 170 200 92 30 240 120 50 53 216 + 147 83 51 73 50 184 106 110 148 17 151 150 45 217 186 85 + 127 98 219 219 58 179 109 101 128 107 237 125 155 240 163 72 + 155 86 240 240 76 201 85 108 121 95 232 137 51 118 153 155 + 149 189 240 240 98 220 141 154 206 178 235 230 152 77 175 209 + + 232 228 239 240 240 240 240 240 240 240 240 207 83 64 155 209 + 240 240 240 240 240 240 240 240 240 240 226 53 186 161 161 175 + 240 240 240 240 240 240 240 240 240 240 180 50 217 232 119 77 + 240 240 240 240 240 240 240 240 240 238 237 120 45 155 51 152 + 238 240 240 240 240 240 240 240 240 237 226 237 150 124 129 227 + 240 240 240 240 240 240 240 240 240 240 223 29 150 237 240 236 + 237 240 240 240 240 240 240 240 240 240 228 72 5 109 108 191 + 240 240 240 240 240 240 240 240 240 240 238 197 142 128 122 207 + 235 240 240 240 240 240 240 240 240 174 147 169 101 92 109 152 + 240 240 240 240 240 240 240 240 240 225 115 113 113 121 98 135 + 234 240 240 240 240 240 240 240 240 216 208 187 184 180 210 219 + 240 240 240 240 240 240 240 240 240 186 75 62 53 58 76 98 + 233 240 240 240 240 240 240 240 240 240 231 213 78 222 240 240 + 240 240 240 240 240 240 240 240 240 240 238 181 47 220 240 240 + 235 240 240 240 240 240 240 240 240 149 107 62 63 84 71 186 + 234 229 227 240 236 234 236 231 229 225 150 150 149 121 149 149 + 240 240 240 240 240 240 240 240 240 240 240 216 85 73 164 212 + 240 240 240 240 240 240 240 240 240 240 226 53 186 170 166 176 + 240 240 240 240 240 240 240 240 240 240 180 50 217 240 120 77 + 240 240 240 240 240 240 240 240 240 240 240 120 52 156 51 152 + 238 240 240 240 240 240 240 240 240 240 237 237 152 125 137 230 + 240 240 240 240 240 240 240 240 240 240 223 29 150 236 240 236 + 236 240 240 240 240 240 240 240 240 240 232 78 5 100 99 192 + 240 240 240 240 240 240 240 240 240 240 240 197 142 128 121 207 + 235 240 240 240 240 240 240 240 240 186 162 168 101 92 109 156 + 240 240 240 240 240 240 240 240 240 225 115 103 106 121 109 150 + 234 240 240 240 240 240 240 240 240 216 206 184 184 184 210 220 + 240 240 240 240 240 240 240 240 240 186 72 51 61 65 76 98 + 233 240 240 240 240 240 240 240 240 240 231 213 74 219 240 240 + 240 240 240 240 240 240 240 240 240 240 240 187 51 219 240 240 + 235 240 240 240 240 240 240 240 240 156 117 74 77 89 71 186 + 235 229 227 240 236 234 236 232 229 225 150 150 144 117 148 140 + 233 239 240 240 240 240 240 240 240 240 240 216 85 72 155 209 + 240 240 240 240 240 240 240 240 240 240 226 53 186 163 153 175 + 240 240 240 240 240 240 240 240 240 240 180 50 217 240 118 77 + 240 240 240 240 240 240 240 240 240 240 240 120 45 155 51 152 + 238 240 240 240 240 240 240 240 240 240 240 240 150 125 137 230 + 240 240 240 240 240 240 240 240 240 240 225 30 151 237 232 235 + 236 240 240 240 240 240 240 240 240 240 236 92 17 107 95 178 + 240 240 240 240 240 240 240 240 240 240 238 200 148 128 121 206 + 235 240 240 240 240 240 240 240 240 180 151 170 110 101 108 154 + 240 240 240 240 240 240 240 240 240 225 115 109 106 109 85 141 + 234 240 240 240 240 240 240 240 240 212 205 184 184 179 201 220 + 240 240 240 240 240 240 240 240 240 182 66 47 50 58 76 98 + 233 240 240 240 240 240 240 240 240 240 228 211 73 219 240 240 + 240 240 240 240 240 240 240 240 240 240 238 186 51 219 240 240 + 235 240 240 240 240 240 240 240 240 156 116 84 83 98 86 189 + 234 229 227 240 234 232 235 232 228 225 150 156 147 127 155 149 + + 209 175 77 152 227 236 191 207 152 135 219 98 240 240 186 149 + 155 161 119 51 129 240 108 122 109 98 210 76 240 240 71 149 + 64 161 232 155 124 237 109 128 92 121 180 58 222 220 84 121 + 83 186 217 45 150 150 5 142 101 113 184 53 78 47 63 149 + 207 53 50 120 237 29 72 197 169 113 187 62 213 181 62 150 + 240 226 180 237 226 223 228 238 147 115 208 75 231 238 107 150 + 240 240 240 238 237 240 240 240 174 225 216 186 240 240 149 225 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 229 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 231 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 236 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 234 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 236 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 239 240 240 240 240 240 240 240 240 240 240 240 240 240 240 227 + 228 240 240 240 240 240 240 240 240 240 240 240 240 240 240 229 + 232 240 240 240 238 240 237 240 235 240 234 240 233 240 235 234 + 212 176 77 152 230 236 192 207 156 150 220 98 240 240 186 140 + 164 166 120 51 137 240 99 121 109 109 210 76 240 240 71 148 + 73 170 240 156 125 236 100 128 92 121 184 65 219 219 89 117 + 85 186 217 52 152 150 5 142 101 106 184 61 74 51 77 144 + 216 53 50 120 237 29 78 197 168 103 184 51 213 187 74 150 + 240 226 180 240 237 223 232 240 162 115 206 72 231 240 117 150 + 240 240 240 240 240 240 240 240 186 225 216 186 240 240 156 225 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 229 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 232 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 236 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 234 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 236 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 227 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 229 + 240 240 240 240 238 240 236 240 235 240 234 240 233 240 235 235 + 209 175 77 152 230 235 178 206 154 141 220 98 240 240 189 149 + 155 153 118 51 137 232 95 121 108 85 201 76 240 240 86 155 + 72 163 240 155 125 237 107 128 101 109 179 58 219 219 98 127 + 85 186 217 45 150 151 17 148 110 106 184 50 73 51 83 147 + 216 53 50 120 240 30 92 200 170 109 184 47 211 186 84 156 + 240 226 180 240 240 225 236 238 151 115 205 66 228 238 116 150 + 240 240 240 240 240 240 240 240 180 225 212 182 240 240 156 225 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 228 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 232 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 235 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 232 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 234 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 + 240 240 240 240 240 240 240 240 240 240 240 240 240 240 240 227 + 239 240 240 240 240 240 240 240 240 240 240 240 240 240 240 229 + 233 240 240 240 238 240 236 240 235 240 234 240 233 240 235 234 + + 149 149 121 149 150 150 225 229 231 236 234 236 240 227 229 234 + 186 71 84 63 62 107 149 240 240 240 240 240 240 240 240 235 + 240 240 220 47 181 238 240 240 240 240 240 240 240 240 240 240 + 240 240 222 78 213 231 240 240 240 240 240 240 240 240 240 233 + 98 76 58 53 62 75 186 240 240 240 240 240 240 240 240 240 + 219 210 180 184 187 208 216 240 240 240 240 240 240 240 240 234 + 135 98 121 113 113 115 225 240 240 240 240 240 240 240 240 240 + 152 109 92 101 169 147 174 240 240 240 240 240 240 240 240 235 + 207 122 128 142 197 238 240 240 240 240 240 240 240 240 240 240 + 191 108 109 5 72 228 240 240 240 240 240 240 240 240 240 237 + 236 240 237 150 29 223 240 240 240 240 240 240 240 240 240 240 + 227 129 124 150 237 226 237 240 240 240 240 240 240 240 240 238 + 152 51 155 45 120 237 238 240 240 240 240 240 240 240 240 240 + 77 119 232 217 50 180 240 240 240 240 240 240 240 240 240 240 + 175 161 161 186 53 226 240 240 240 240 240 240 240 240 240 240 + 209 155 64 83 207 240 240 240 240 240 240 240 240 239 228 232 + 140 148 117 144 150 150 225 229 232 236 234 236 240 227 229 235 + 186 71 89 77 74 117 156 240 240 240 240 240 240 240 240 235 + 240 240 219 51 187 240 240 240 240 240 240 240 240 240 240 240 + 240 240 219 74 213 231 240 240 240 240 240 240 240 240 240 233 + 98 76 65 61 51 72 186 240 240 240 240 240 240 240 240 240 + 220 210 184 184 184 206 216 240 240 240 240 240 240 240 240 234 + 150 109 121 106 103 115 225 240 240 240 240 240 240 240 240 240 + 156 109 92 101 168 162 186 240 240 240 240 240 240 240 240 235 + 207 121 128 142 197 240 240 240 240 240 240 240 240 240 240 240 + 192 99 100 5 78 232 240 240 240 240 240 240 240 240 240 236 + 236 240 236 150 29 223 240 240 240 240 240 240 240 240 240 240 + 230 137 125 152 237 237 240 240 240 240 240 240 240 240 240 238 + 152 51 156 52 120 240 240 240 240 240 240 240 240 240 240 240 + 77 120 240 217 50 180 240 240 240 240 240 240 240 240 240 240 + 176 166 170 186 53 226 240 240 240 240 240 240 240 240 240 240 + 212 164 73 85 216 240 240 240 240 240 240 240 240 240 240 240 + 149 155 127 147 156 150 225 228 232 235 232 234 240 227 229 234 + 189 86 98 83 84 116 156 240 240 240 240 240 240 240 240 235 + 240 240 219 51 186 238 240 240 240 240 240 240 240 240 240 240 + 240 240 219 73 211 228 240 240 240 240 240 240 240 240 240 233 + 98 76 58 50 47 66 182 240 240 240 240 240 240 240 240 240 + 220 201 179 184 184 205 212 240 240 240 240 240 240 240 240 234 + 141 85 109 106 109 115 225 240 240 240 240 240 240 240 240 240 + 154 108 101 110 170 151 180 240 240 240 240 240 240 240 240 235 + 206 121 128 148 200 238 240 240 240 240 240 240 240 240 240 240 + 178 95 107 17 92 236 240 240 240 240 240 240 240 240 240 236 + 235 232 237 151 30 225 240 240 240 240 240 240 240 240 240 240 + 230 137 125 150 240 240 240 240 240 240 240 240 240 240 240 238 + 152 51 155 45 120 240 240 240 240 240 240 240 240 240 240 240 + 77 118 240 217 50 180 240 240 240 240 240 240 240 240 240 240 + 175 153 163 186 53 226 240 240 240 240 240 240 240 240 240 240 + 209 155 72 85 216 240 240 240 240 240 240 240 240 240 239 233 + diff --git a/res/values/strings.xml b/res/values/strings.xml new file mode 100644 index 0000000..16e091e --- /dev/null +++ b/res/values/strings.xml @@ -0,0 +1,4 @@ + + + NyARToolkitAndroidDemo + diff --git a/src/com/tomgibara/android/camera/BitmapCamera.java b/src/com/tomgibara/android/camera/BitmapCamera.java new file mode 100644 index 0000000..8b47530 --- /dev/null +++ b/src/com/tomgibara/android/camera/BitmapCamera.java @@ -0,0 +1,60 @@ +package com.tomgibara.android.camera; + +import android.graphics.Bitmap; +import android.graphics.Canvas; +import android.graphics.Paint; +import android.graphics.Rect; + +/** + * A CameraSource implementation that repeatedly captures a single bitmap. + * + * @author Tom + * + */ + +public class BitmapCamera implements CameraSource { + + private final Bitmap bitmap; + private final Rect bounds; + private final Paint paint = new Paint(); + + public BitmapCamera(Bitmap bitmap, int width, int height) { + this.bitmap = bitmap; + bounds = new Rect(0, 0, width, height); + + paint.setFilterBitmap(true); + paint.setAntiAlias(true); + } + + @Override + public int getWidth() { + return bounds.right; + } + + @Override + public int getHeight() { + return bounds.bottom; + } + + @Override + public boolean open() { + return true; + } + + @Override + public boolean capture(Canvas canvas) { + if ( + bounds.right == bitmap.width() && + bounds.bottom == bitmap.height()) { + canvas.drawBitmap(bitmap, 0, 0, null); + } else { + canvas.drawBitmap(bitmap, null, bounds, paint); + } + return true; + } + + @Override + public void close() { + /* nothing to do */ + } +} diff --git a/src/com/tomgibara/android/camera/CameraSource.java b/src/com/tomgibara/android/camera/CameraSource.java new file mode 100644 index 0000000..24ce0d5 --- /dev/null +++ b/src/com/tomgibara/android/camera/CameraSource.java @@ -0,0 +1,61 @@ +package com.tomgibara.android.camera; + +import android.graphics.Canvas; + +/** + * Provides a simple abstraction for obtaining preview captures from a camera + * on the Android platform. This interface intended to be used temporarily while + * the Google Android SDK fails to support camera capture from desktop devices + * (webcams etc). + * + * @author Tom Gibara + */ + +public interface CameraSource { + + static final String LOG_TAG = "camera"; + + /** + * Open the camera source for subsequent use via calls to capture(). + * + * @return true if the camera source was successfully opened. + */ + + boolean open(); + + /** + * Close the camera source. Calling close on a closed CameraSource is + * permitted but has no effect. The camera source may be reopened after + * being closed. + */ + + void close(); + + /** + * The width of the captured image. + * + * @return the width of the capture in pixels + */ + + int getWidth(); + + /** + * The height of the captured image. + * + * @return the height of the capture in pixels + */ + + int getHeight(); + + /** + * Attempts to render the current camera view onto the supplied canvas. + * The capture will be rendered into the rectangle (0,0,width,height). + * Outstanding transformations on the canvas may alter this. + * + * @param canvas the canvas to which the captured pixel data will be written + * @return true iff a frame was successfully written to the canvas + */ + + boolean capture(Canvas canvas); + +} diff --git a/src/com/tomgibara/android/camera/GenuineCamera.java b/src/com/tomgibara/android/camera/GenuineCamera.java new file mode 100644 index 0000000..ecd67b0 --- /dev/null +++ b/src/com/tomgibara/android/camera/GenuineCamera.java @@ -0,0 +1,76 @@ +package com.tomgibara.android.camera; + +import android.graphics.Canvas; +import android.hardware.CameraDevice; + +/** + * A CameraSource implementation that obtains its bitmaps directly from the + * device camera. + * + * @author Tom Gibara + * + */ + +public class GenuineCamera implements CameraSource { + + private final int width; + private final int height; + + private CameraDevice device = null; + + public GenuineCamera(int width, int height) { + this.width = width; + this.height = height; + } + + @Override + public int getWidth() { + return width; + } + + @Override + public int getHeight() { + return height; + } + + @Override + public boolean open() { + if (device != null) return true; + device = CameraDevice.open(); + if (device == null) return false; + + //parameters for the device mostly as specified in sample app + CameraDevice.CaptureParams param = new CameraDevice.CaptureParams(); + param.type = 1; // preview + param.srcWidth = 1280; + param.srcHeight = 960; + param.leftPixel = 0; + param.topPixel = 0; + param.outputWidth = width; + param.outputHeight = height; + param.dataFormat = 2; // RGB_565 + + //attempt to configure the device here + if (!device.setCaptureParams(param)) { + device.close(); + device = null; + return false; + } + + return true; + } + + @Override + public void close() { + if (device == null) return; + device.close(); + device = null; + } + + @Override + public boolean capture(Canvas canvas) { + if (device == null) return false; + return device.capture(canvas); + } + +} diff --git a/src/com/tomgibara/android/camera/HttpCamera.java b/src/com/tomgibara/android/camera/HttpCamera.java new file mode 100644 index 0000000..0f8c124 --- /dev/null +++ b/src/com/tomgibara/android/camera/HttpCamera.java @@ -0,0 +1,128 @@ +package com.tomgibara.android.camera; + +import java.io.IOException; +import java.io.InputStream; +import java.net.HttpURLConnection; +import java.net.URL; +import java.net.URLConnection; + +import android.graphics.Bitmap; +import android.graphics.BitmapFactory; +import android.graphics.Canvas; +import android.graphics.Paint; +import android.graphics.Rect; +import android.util.Log; + +/** + * A CameraSource implementation that obtains its bitmaps via an HTTP request + * to a URL. + * + * @author Tom Gibara + * + */ + +public class HttpCamera implements CameraSource { + + + private static final int CONNECT_TIMEOUT = 1000; + private static final int SOCKET_TIMEOUT = 1000; + + private final String url; + private final Rect bounds; + private final boolean preserveAspectRatio; + private final Paint paint = new Paint(); + + public HttpCamera(String url, int width, int height, boolean preserveAspectRatio) { + this.url = url; + bounds = new Rect(0, 0, width, height); + this.preserveAspectRatio = preserveAspectRatio; + + paint.setFilterBitmap(true); + paint.setAntiAlias(true); + } + + @Override + public int getWidth() { + return bounds.right; + } + + @Override + public int getHeight() { + return bounds.bottom; + } + + @Override + public boolean open() { + /* nothing to do */ + return true; + } + + @Override + public boolean capture(Canvas canvas) { + if (canvas == null) throw new IllegalArgumentException("null canvas"); + try { + Bitmap bitmap = null; + InputStream in = null; + int response = -1; + try { + //we use URLConnection because it's anticipated that it is lighter-weight than HttpClient + //NOTE: At this time, neither properly support connect timeouts + //as a consequence, this implementation will hang on a failure to connect + URL url = new URL(this.url); + URLConnection conn = url.openConnection(); + if (!(conn instanceof HttpURLConnection)) throw new IOException("Not an HTTP connection."); + HttpURLConnection httpConn = (HttpURLConnection) conn; + httpConn.setAllowUserInteraction(false); + httpConn.setConnectTimeout(CONNECT_TIMEOUT); + httpConn.setReadTimeout(SOCKET_TIMEOUT); + httpConn.setInstanceFollowRedirects(true); + httpConn.setRequestMethod("GET"); + httpConn.connect(); + response = httpConn.getResponseCode(); + if (response == HttpURLConnection.HTTP_OK) { + in = httpConn.getInputStream(); + bitmap = BitmapFactory.decodeStream(in); + } + } finally { + if (in != null) try { + in.close(); + } catch (IOException e) { + /* ignore */ + } + } + + if (bitmap == null) throw new IOException("Response Code: " + response); + + //render it to canvas, scaling if necessary + if ( + bounds.right == bitmap.width() && + bounds.bottom == bitmap.height()) { + canvas.drawBitmap(bitmap, 0, 0, null); + } else { + Rect dest; + if (preserveAspectRatio) { + dest = new Rect(bounds); + dest.bottom = bitmap.height() * bounds.right / bitmap.width(); + dest.offset(0, (bounds.bottom - dest.bottom)/2); + } else { + dest = bounds; + } + canvas.drawBitmap(bitmap, null, dest, paint); + } + + } catch (RuntimeException e) { + Log.i(LOG_TAG, "Failed to obtain image over network", e); + return false; + } catch (IOException e) { + Log.i(LOG_TAG, "Failed to obtain image over network", e); + return false; + } + return true; + } + + @Override + public void close() { + /* nothing to do */ + } + +} diff --git a/src/com/tomgibara/android/camera/SocketCamera.java b/src/com/tomgibara/android/camera/SocketCamera.java new file mode 100644 index 0000000..28f7403 --- /dev/null +++ b/src/com/tomgibara/android/camera/SocketCamera.java @@ -0,0 +1,432 @@ +package com.tomgibara.android.camera; + +import java.io.IOException; +import java.io.InputStream; +import java.net.InetSocketAddress; +import java.net.Socket; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.nio.FloatBuffer; +import java.nio.IntBuffer; + +import javax.microedition.khronos.opengles.GL10; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.NyARCode; +import jp.nyatla.nyartoolkit.core.raster.NyARRaster_RGB; +import jp.nyatla.nyartoolkit.jogl.utils.GLNyARParam; +import jp.nyatla.nyartoolkit.jogl.utils.GLNyARSingleDetectMarker; +import android.graphics.Bitmap; +import android.graphics.BitmapFactory; +import android.graphics.Canvas; +import android.graphics.OpenGLContext; +import android.graphics.Paint; +import android.graphics.Rect; +import android.util.Log; +import android.view.SurfaceHolder; + +/** + * A CameraSource implementation that obtains its bitmaps via a TCP connection + * to a remote host on a specified address/port. + * + * Originally wrote by Tom Gibara + * + * + * Modified for detecting ARMarker and drawing 3D cube. + * + * @author Shiva + * + */ + +public class SocketCamera implements CameraSource { + + private static final int SOCKET_TIMEOUT = 5000; + + private final String address; + private final int port; + private final Rect bounds; + private final boolean preserveAspectRatio; + private final Paint paint = new Paint(); + private final Paint paintBlue = new Paint(); + + private GLNyARSingleDetectMarker nya; + private NyARRaster_RGB raster; + private GLNyARParam ar_param; + // private NyARCode ar_code; + + private SurfaceHolder mHolder; + + private final Bitmap fixedBitmap; + private final Cube mCube = new Cube(); + + private int w = -1; + private int h = -1; + private int[] pixels; + private byte[] buf; + private float[] resultf; + + private static final boolean LOCAL = false; + private static final boolean TEXT = false; + private static final boolean DETECT = true; + + public SocketCamera(String address, int port, int width, int height, + boolean preserveAspectRatio, GLNyARParam ar_param, + NyARCode ar_code, SurfaceHolder holder, Bitmap fixedBitmap) { + this.address = address; + this.port = port; + bounds = new Rect(0, 0, width, height); + this.preserveAspectRatio = preserveAspectRatio; + + paint.setColor(0xFFFF0000); + paint.setFilterBitmap(true); + paint.setAntiAlias(true); + + paintBlue.setColor(0xFF0000FF); + paintBlue.setFilterBitmap(true); + paintBlue.setAntiAlias(true); + + this.ar_param = ar_param; + // this.ar_code = ar_code; + try { + nya = new GLNyARSingleDetectMarker(ar_param, ar_code, 80.0); + nya.setContinueMode(true); + } catch (NyARException e) { + e.printStackTrace(); + } + + mHolder = holder; + + this.fixedBitmap = fixedBitmap; + + } + + @Override + public int getWidth() { + return bounds.right; + } + + @Override + public int getHeight() { + return bounds.bottom; + } + + @Override + public boolean open() { + /* nothing to do */ + return true; + } + + @Override + public boolean capture(Canvas canvas) { + if (canvas == null) { + throw new IllegalArgumentException("null canvas"); + } + + int[] pixels = this.pixels; + byte[] buf = this.buf; + float[] resultf = this.resultf; + + int width = 320; + int height = 240; + if (this.w != width || this.h != height || buf == null) { + this.w = width; + this.h = height; + pixels = new int[w * h]; + buf = new byte[pixels.length * 3]; + resultf = new float[16]; + } + int w = this.w; + int h = this.h; + + Socket socket = null; + try { + Bitmap bitmap = null; + if (!LOCAL) { + socket = new Socket(); + socket.setSoTimeout(SOCKET_TIMEOUT); + socket.connect(new InetSocketAddress(address, port), + SOCKET_TIMEOUT); + // obtain the bitmap + InputStream in = socket.getInputStream(); + // DataInputStream dis = new DataInputStream( + // new BufferedInputStream(in, buf.length)); + // dis.readFully(buf); + + bitmap = BitmapFactory.decodeStream(in); + // bitmap = BitmapFactory.decodeByteArray(buf, 0, buf.length); + } else { + bitmap = fixedBitmap; + } + + // GL init + OpenGLContext glc = new OpenGLContext(OpenGLContext.DEPTH_BUFFER); + glc.makeCurrent(mHolder); + GL10 gl = (GL10) (glc.getGL()); + gl.glHint(GL10.GL_PERSPECTIVE_CORRECTION_HINT, GL10.GL_FASTEST); + + gl.glViewport(0, 0, w, h); + float ratio = (float) h / w; + gl.glMatrixMode(GL10.GL_PROJECTION); + gl.glLoadIdentity(); + gl.glFrustumf(-1.0f, 1.0f, -ratio, ratio, 5.0f, 40.0f); + // 見る位置 + gl.glMatrixMode(GL10.GL_MODELVIEW); + gl.glLoadIdentity(); + gl.glTranslatef(0.0f, 0.0f, -10.0f); + + gl.glDisable(GL10.GL_DITHER); + gl.glActiveTexture(GL10.GL_TEXTURE0); + gl.glBindTexture(GL10.GL_TEXTURE_2D, 0); + gl.glTexParameterx(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_WRAP_S, + GL10.GL_CLAMP_TO_EDGE); + gl.glTexParameterx(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_WRAP_T, + GL10.GL_CLAMP_TO_EDGE); + gl.glTexParameterx(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_MAG_FILTER, + GL10.GL_NEAREST); + gl.glTexParameterx(GL10.GL_TEXTURE_2D, GL10.GL_TEXTURE_MIN_FILTER, + GL10.GL_NEAREST); + gl.glTexEnvx(GL10.GL_TEXTURE_ENV, GL10.GL_TEXTURE_ENV_MODE, + GL10.GL_REPLACE); + + gl.glColor4f(0.7f, 0.7f, 0.7f, 1.0f); + gl.glEnableClientState(GL10.GL_VERTEX_ARRAY); + gl.glEnableClientState(GL10.GL_COLOR_ARRAY); + gl.glEnable(GL10.GL_CULL_FACE); + gl.glShadeModel(GL10.GL_SMOOTH); + gl.glEnable(GL10.GL_DEPTH_TEST); + + boolean is_marker_exist = false; + // long time = 0; + // double[][] atm = null; + + if (DETECT) { + // time = System.currentTimeMillis(); + // ここで処理 + + bitmap.getPixels(pixels, 0, w, 0, 0, w, h); + + for (int i = 0; i < pixels.length; i++) { + int argb = pixels[i]; + // byte a = (byte) (argb & 0xFF000000 >> 24); + byte r = (byte) (argb & 0x00FF0000 >> 16); + byte g = (byte) (argb & 0x0000FF00 >> 8); + byte b = (byte) (argb & 0x000000FF); + buf[i * 3] = r; + buf[i * 3 + 1] = g; + buf[i * 3 + 2] = b; + } + // Log.d("nyar", "Time to copy image : " + // + (System.currentTimeMillis() - time) + " ms"); + + if (raster == null) { + raster = new NyARRaster_RGB(); + } + // time = System.currentTimeMillis(); + NyARRaster_RGB.wrap(raster, buf, w, h); + // マーカー検出 + try { + // Log.d("nyar", "detecting marker"); + is_marker_exist = nya.detectMarkerLite(raster, 100); + // Log.d("nyar", "marker detected"); + + // if (is_marker_exist) { + // 変換行列を取得 + // atm = nya.getTransmationMatrix().getArray(); + // } + } catch (NyARException e) { + Log.e("nyar", "marker detection failed", e); + return false; + } + // Log.d("nyar", "Time to detect marker : " + // + (System.currentTimeMillis() - time) + " ms"); + } else { + gl.glMatrixMode(GL10.GL_MODELVIEW); + gl.glLoadIdentity(); + // int num = (int) (System.currentTimeMillis() % 3) - 1; + int num = 0; + gl.glTranslatef(0.0f, 0.0f, -10.0f); + gl.glRotatef(20.0f * num, 0, 0, 1); + } + if (is_marker_exist) { + // Log.d("nyar", "marker exist"); + // // Projection transformation. + gl.glMatrixMode(GL10.GL_PROJECTION); + gl.glLoadIdentity(); + gl.glLoadMatrixf(ar_param.getCameraFrustumRHf(), 0); + gl.glMatrixMode(GL10.GL_MODELVIEW); + // Viewing transformation. + gl.glLoadIdentity(); + nya.getCameraViewRH(resultf); + // int num = (int) (System.currentTimeMillis() % 3) - 1; + // gl.glTranslatef(0.0f, 2.5f, -20.0f); + gl.glLoadMatrixf(resultf, 0); + // gl.glTranslatef(0.0f, 0.0f, -10.0f); + // gl.glRotatef(20.0f, 1, 0, 0); + // gl.glScalef(0.5f, 0.5f, 1.0f); + // gl.glRotatef(20.0f * num, 0, 0, 1); + + gl.glFinish(); + } else { + // Log.d("nyar", "marker does not exist"); + } + + // time = System.currentTimeMillis(); + gl.glClearColor(1, 1, 1, 1); + gl.glClear(GL10.GL_COLOR_BUFFER_BIT | GL10.GL_DEPTH_BUFFER_BIT); + gl.glFinish(); + // render it to canvas, scaling if necessary + if (bounds.right == bitmap.width() + && bounds.bottom == bitmap.height()) { + canvas.drawBitmap(bitmap, 0, 0, null); + } else { + // Log.d("nyar", "preserveAspectRation = " + + // preserveAspectRatio); + Rect dest; + if (preserveAspectRatio) { + dest = new Rect(bounds); + dest.bottom = bitmap.height() * bounds.right + / bitmap.width(); + dest.offset(0, (bounds.bottom - dest.bottom) / 2); + } else { + dest = bounds; + } + canvas.drawBitmap(bitmap, null, dest, paint); + } + + if (is_marker_exist) { + // Log.d("nyar", "drawing a cube."); + mCube.draw(gl); + // gl.glRotatef(20.0f, 0, 0, 1.0f); + // mCube.draw(gl); + } else if (!DETECT) { + // Log.d("nyar", "drawing a cube without detection."); + mCube.draw(gl); + // Thread.sleep(1000); + } + gl.glFinish(); + // Log.d("nyar", "" + (System.currentTimeMillis() - time) + "ms"); + + if (TEXT) { + String str = null; + // 情報を画面に書く + // g.drawImage(img, 32, 32, this); + if (is_marker_exist) { + // Log.d("nyar", "marker exist and drawing text"); + // str = "マーカー検出 [" + nya.getConfidence() + "]"; + // canvas.drawText(str, 32, 100, paint); + // for (int i = 0; i < 3; i++) { + // for (int i2 = 0; i2 < 4; i2++) { + // str = "[" + i + "][" + i2 + "]" + atm[i][i2]; + // canvas.drawText(str, 32, 0 + (1 + i2 * 3 + i) * 16, + // paint); + // } + // } + + for (int i = 0; i < 4; i++) { + for (int i2 = 0; i2 < 4; i2++) { + str = "[" + i + "][" + i2 + "]" + + resultf[i * 4 + i2]; + canvas.drawText(str, 32, 0 + (1 + i * 4 + i2) * 16, + paint); + } + } + float[] arp = ar_param.getCameraFrustumRHf(); + for (int i = 0; i < 4; i++) { + for (int i2 = 0; i2 < 4; i2++) { + str = "[" + i + "][" + i2 + "]" + arp[i * 4 + i2]; + canvas.drawText(str, 152, + 0 + (1 + i * 4 + i2) * 16, paintBlue); + } + } + } else { + // Log.d("nyar", "marker does not exist"); + // g.drawString("マーカー未検出:", 32, 100); + str = "マーカー未検出"; + canvas.drawText(str, 32, 100, paint); + } + } // end if + } catch (Exception e) { + Log.i(LOG_TAG, "Failed to obtain image over network", e); + return false; + } finally { + try { + if (socket != null) { + socket.close(); + } + } catch (IOException e) { + /* ignore */ + } + } + return true; + } + + @Override + public void close() { + /* nothing to do */ + } + + class Cube { + + private FloatBuffer mVertexBuffer; + private IntBuffer mColorBuffer; + private ByteBuffer mIndexBuffer; + + public Cube() { + float fone = 0.5f; + float[] vertices = { fone, fone, fone, fone, -fone, fone, -fone, + -fone, fone, -fone, fone, fone, fone, fone, -fone, fone, + -fone, -fone, -fone, -fone, -fone, -fone, fone, -fone }; + + int one = 0x00010000; + int[] colors = { 0, 0, 0, one, one, 0, 0, one, one, one, 0, one, 0, + one, 0, one, 0, 0, one, one, one, 0, one, one, one, one, + one, one, 0, one, one, one }; + + byte[] indices = new byte[] { 0, 4, 5, 0, 5, 1, 1, 5, 6, 1, 6, 2, + 2, 6, 7, 2, 7, 3, 3, 7, 4, 3, 4, 0, 4, 7, 6, 4, 6, 5, 3, 0, + 1, 3, 1, 2 }; + // indices = new byte[] { 3, 2, 1, 0, 2, 3, 7, 6, 0, 1, 5, 4, 3, 0, + // 4, + // 7, 1, 2, 6, 5, 4, 5, 6, 7 }; + + // Buffers to be passed to gl*Pointer() functions + // must be direct, i.e., they must be placed on the + // native heap where the garbage collector cannot + // move them. + // + // Buffers with multi-byte datatypes (e.g., short, int, float) + // must have their byte order set to native order + + ByteBuffer vbb = ByteBuffer.allocateDirect(vertices.length * 4); + vbb.order(ByteOrder.nativeOrder()); + mVertexBuffer = vbb.asFloatBuffer(); + mVertexBuffer.put(vertices); + mVertexBuffer.position(0); + + ByteBuffer cbb = ByteBuffer.allocateDirect(colors.length * 4); + cbb.order(ByteOrder.nativeOrder()); + mColorBuffer = cbb.asIntBuffer(); + mColorBuffer.put(colors); + mColorBuffer.position(0); + + mIndexBuffer = ByteBuffer.allocateDirect(indices.length); + mIndexBuffer.put(indices); + mIndexBuffer.position(0); + } + + public void draw(GL10 gl) { + + gl.glTranslatef(0.0f, 0.0f, 0.5f); // Place base of cube on marker + + gl.glFrontFace(GL10.GL_CW); + gl.glVertexPointer(3, GL10.GL_FLOAT, 0, mVertexBuffer); + gl.glColorPointer(4, GL10.GL_FIXED, 0, mColorBuffer); + gl.glDrawElements(GL10.GL_TRIANGLES, 36, GL10.GL_UNSIGNED_BYTE, + mIndexBuffer); + + gl.glTranslatef(0.0f, 0.0f, -0.5f); // Place base of cube on marker + } + + } + +} diff --git a/src/jp/ac/kyutech/ai/ylab/shiva/NyARToolkitAndroidActivity.java b/src/jp/ac/kyutech/ai/ylab/shiva/NyARToolkitAndroidActivity.java new file mode 100644 index 0000000..6c79802 --- /dev/null +++ b/src/jp/ac/kyutech/ai/ylab/shiva/NyARToolkitAndroidActivity.java @@ -0,0 +1,321 @@ +package jp.ac.kyutech.ai.ylab.shiva; + +import java.io.InputStream; + +import jp.nyatla.nyartoolkit.core.NyARCode; +import jp.nyatla.nyartoolkit.jogl.utils.GLNyARParam; +import android.app.Activity; +import android.content.Context; +import android.graphics.Bitmap; +import android.graphics.BitmapFactory; +import android.graphics.Canvas; +import android.os.Bundle; +import android.util.Log; +import android.view.SurfaceHolder; +import android.view.SurfaceView; +import android.view.View; +import android.view.ViewGroup; +import android.view.Window; +import android.view.View.OnClickListener; +import android.widget.Button; +import android.widget.EditText; +import android.widget.LinearLayout; +import android.widget.TableLayout; +import android.widget.TableRow; +import android.widget.TextView; + +import com.tomgibara.android.camera.CameraSource; +import com.tomgibara.android.camera.SocketCamera; + +public class NyARToolkitAndroidActivity extends Activity { + + private final int WC = ViewGroup.LayoutParams.WRAP_CONTENT; + private final int FP = ViewGroup.LayoutParams.FILL_PARENT; + + private Preview mPreview; + + private Button button; + + private boolean connected; + + private TextView infoText; + + @Override + protected void onCreate(Bundle icicle) { + super.onCreate(icicle); + + Log.d("trace", "onCreate"); + + // Hide the window title. + requestWindowFeature(Window.FEATURE_NO_TITLE); + + LinearLayout linearLayout = new LinearLayout(this); + linearLayout.setOrientation(LinearLayout.VERTICAL); + setContentView(linearLayout); + + // Create our Preview view and set it as the content of our + // Activity + mPreview = new Preview(this); + linearLayout.addView(mPreview, new LinearLayout.LayoutParams(WC, WC)); + + infoText = new TextView(this); + infoText.setText("Information will be shown in here."); + linearLayout.addView(infoText, new LinearLayout.LayoutParams(FP, WC)); + + TableLayout tableLayout = new TableLayout(this); + tableLayout.setColumnStretchable(1, true); + linearLayout + .addView(tableLayout, new LinearLayout.LayoutParams(WC, WC)); + + TextView hostLabel = new TextView(this); + hostLabel.setText("Host : "); + + final EditText host = new EditText(this); + host.setText("192.168.11.5"); + + TableRow tableRow1 = new TableRow(this); + tableRow1.addView(hostLabel); + tableRow1.addView(host, new TableRow.LayoutParams(FP, WC)); + + TextView portLabel = new TextView(this); + portLabel.setText("Port : "); + + final EditText port = new EditText(this); + port.setText("9889"); + + TableRow tableRow2 = new TableRow(this); + tableRow2.addView(portLabel); + tableRow2.addView(port, new TableRow.LayoutParams(FP, WC)); + + button = new Button(this); + button.setOnClickListener(new OnClickListener() { + @Override + public void onClick(View v) { + if (connected) { + infoText.setText("Disconnected"); + mPreview.pause(); + button.setText("Connect"); + } else { + String hostStr = host.getText().toString(); + String portStr = port.getText().toString(); + try { + int p = Integer.parseInt(portStr); + + mPreview.set(hostStr, p); + } catch (Exception e) { + infoText.setText("Illegal Argument"); + return; + } + + infoText.setText("Connecting to " + hostStr + " on port " + + portStr); + mPreview.resume(); + + button.setText("Disconnect"); + } + } + }); + button.setText("Connect"); + + TableRow tableRow3 = new TableRow(this); + TableRow.LayoutParams trl2 = new TableRow.LayoutParams(FP, WC); + trl2.span = 2; + tableRow3.addView(button, trl2); + + tableLayout.addView(tableRow1, new TableLayout.LayoutParams(FP, WC)); + tableLayout.addView(tableRow2, new TableLayout.LayoutParams(FP, WC)); + tableLayout.addView(tableRow3, new TableLayout.LayoutParams(FP, WC)); + + } + + @Override + protected void onResume() { + Log.d("trace", "onRegume"); + // Because the CameraDevice object is not a shared resource, + // it's very important to release it when the activity is paused. + super.onResume(); + + button.setText("Connect"); + connected = false; + + } + + @Override + protected void onPause() { + Log.d("trace", "onPause"); + // Start Preview again when we resume. + super.onPause(); + + mPreview.pause(); + connected = false; + } + + class Preview extends SurfaceView implements SurfaceHolder.Callback { + SurfaceHolder mHolder; + private PreviewThread mPreviewThread; + private boolean mHasSurface; + + private GLNyARParam ar_param = new GLNyARParam(); + private NyARCode ar_code = new NyARCode(16, 16); + + // String host = "133.17.159.104"; + String host = "192.168.11.5"; + + int port = 9889; + + private Bitmap fixedBitmap; + + public void set(String host, int port) { + this.host = host; + this.port = port; + } + + Preview(Context context) { + super(context); + + // Install a SurfaceHolder.Callback so we get notified when the + // underlying surface is created and destroyed. + mHolder = getHolder(); + mHolder.addCallback(this); + mHasSurface = false; + + // In this example, we hardcode the size of the preview. In a real + // application this should be more dynamic. This guarantees that + // the uderlying surface will never change size. + mHolder.setFixedSize(320, 240); + + InputStream in = getResources().openRawResource(R.raw.marker); + fixedBitmap = BitmapFactory.decodeStream(in); + + // NyARToolkitの準備 + try { + ar_param.loadFromARFile(getResources().openRawResource( + R.raw.camera_para)); + ar_param.changeSize(320, 240); + ar_code.loadFromARFile(getResources().openRawResource( + R.raw.patt)); + Log.i("nyar", "resources have been loaded"); + } catch (Exception e) { + Log.e("nyar", "resource loading failed", e); + } + } + + public void resume() { + // We do the actual acquisition in a separate thread. Create it now. + if (mPreviewThread == null) { + mPreviewThread = new PreviewThread(); + // If we already have a surface, just start the thread now too. + if (mHasSurface == true) { + mPreviewThread.start(); + } + connected = true; + } + + } + + public void pause() { + // Stop Preview. + if (mPreviewThread != null) { + mPreviewThread.requestExitAndWait(); + mPreviewThread = null; + + connected = false; + } + } + + public void surfaceCreated(SurfaceHolder holder) { + // The Surface has been created, start our main acquisition thread. + mHasSurface = true; + if (mPreviewThread != null) { + mPreviewThread.start(); + } + } + + public void surfaceDestroyed(SurfaceHolder holder) { + // Surface will be destroyed when we return. Stop the preview. + mHasSurface = false; + pause(); + } + + public void surfaceChanged(SurfaceHolder holder, int format, int w, + int h) { + // Surface size or format has changed. This should not happen in + // this + // example. + } + + // ---------------------------------------------------------------------- + + class PreviewThread extends Thread { + private boolean mDone; + + PreviewThread() { + super(); + mDone = false; + } + + @Override + public void run() { + // We first open the CameraDevice and configure it. + // CameraDevice camera = CameraDevice.open(); + // if (camera != null) { + // CameraDevice.CaptureParams param = new + // CameraDevice.CaptureParams(); + // param.type = 1; // preview + // param.srcWidth = 1280; + // param.srcHeight = 960; + // param.leftPixel = 0; + // param.topPixel = 0; + // param.outputWidth = 320; + // param.outputHeight = 240; + // param.dataFormat = 2; // RGB_565 + // camera.setCaptureParams(param); + // } + + Log.d("trace", "connecting to host " + host + " port" + +port); + CameraSource cs = new SocketCamera(host, port, 320, 240, true, + ar_param, ar_code, mHolder, fixedBitmap); + if (!cs.open()) { + /* deal with failure to obtain camera */ + Log.d("trace", "couldn'd open camera source"); + return; + } + // This is our main acquisition thread's loop, we go until + // asked to quit. + SurfaceHolder holder = mHolder; + while (!mDone) { + // Lock the surface, this returns a Canvas that can + // be used to render into. + Canvas canvas = holder.lockCanvas(); + + // // Capture directly into the Surface + // if (camera != null) { + // camera.capture(canvas); + // } + + if (cs != null) { + cs.capture(canvas); // capture the frame onto the canvas + } + + // And finally unlock and post the surface. + holder.unlockCanvasAndPost(canvas); + } + + // Make sure to release the CameraDevice + if (cs != null) { + cs.close(); + } + } + + public void requestExitAndWait() { + // don't call this from PreviewThread thread or it a guaranteed + // deadlock! + mDone = true; + try { + join(); + } catch (InterruptedException ex) { + } + } + } + } +} diff --git a/src/jp/nyatla/nyartoolkit/NyARException.java b/src/jp/nyatla/nyartoolkit/NyARException.java new file mode 100644 index 0000000..134ebbd --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/NyARException.java @@ -0,0 +1,49 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit; + +public class NyARException extends Exception { + private static final long serialVersionUID = 1L; + public NyARException(){ + super(); + } + public NyARException(Exception e){ + super(e); + } + public NyARException(String m){ + super(m); + } + public static void trap(String m) throws NyARException + { + throw new NyARException("トラップ:"+m); + } +} diff --git a/src/jp/nyatla/nyartoolkit/base/Config.java b/src/jp/nyatla/nyartoolkit/base/Config.java new file mode 100644 index 0000000..add7b1b --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/base/Config.java @@ -0,0 +1,75 @@ +package jp.nyatla.nyartoolkit.base; + +public class Config { + //Version系情報 +// public static final int AR_HEADER_VERSION_MAJOR=2; //#define AR_HEADER_VERSION_MAJOR 2 +// public static final int AR_HEADER_VERSION_MINOR=72;//#define AR_HEADER_VERSION_MINOR 72 +// +// public static final int AR_HEADER_VERSION_TINY=0;//#define AR_HEADER_VERSION_TINY 0 +// +// public static final int AR_HEADER_VERSION_BUILD=0;//#define AR_HEADER_VERSION_BUILD 0 +// +// public static final String AR_HEADER_VERSION_STRING="2.72.0";//#define AR_HEADER_VERSION_STRING "2.72.0" +// +// public static final boolean AR_HAVE_HEADER_VERSION_2=true;//#define AR_HAVE_HEADER_VERSION_2 +// public static final boolean AR_HAVE_HEADER_VERSION_2_72=true;//#define AR_HAVE_HEADER_VERSION_2_72 + + + /*ビデオ入力系？*/ + /*------------------------------------------------------------*/ + +// public static final int AR_DRAW_BY_GL_DRAW_PIXELS =0;//#define AR_DRAW_BY_GL_DRAW_PIXELS 0 +// public static final int AR_DRAW_BY_TEXTURE_MAPPING=1;//#define AR_DRAW_BY_TEXTURE_MAPPING 1 +// public static final int AR_DRAW_TEXTURE_FULL_IMAGE=0;//#define AR_DRAW_TEXTURE_FULL_IMAGE 0 +// public static final int AR_DRAW_TEXTURE_HALF_IMAGE=1;//#define AR_DRAW_TEXTURE_HALF_IMAGE 1 +// public static final int AR_IMAGE_PROC_IN_FULL=0;//#define AR_IMAGE_PROC_IN_FULL 0 +// public static final int AR_IMAGE_PROC_IN_HALF=1;//#define AR_IMAGE_PROC_IN_HALF 1 +// public static final int AR_FITTING_TO_IDEAL=0;//#define AR_FITTING_TO_IDEAL 0 +// public static final int AR_FITTING_TO_INPUT=1;//#define AR_FITTING_TO_INPUT 1 + +// public static final int AR_TEMPLATE_MATCHING_COLOR=0;//#define AR_TEMPLATE_MATCHING_COLOR 0 +// public static final int AR_TEMPLATE_MATCHING_BW=1;//#define AR_TEMPLATE_MATCHING_BW 1 +// public static final int AR_MATCHING_WITHOUT_PCA=0;//#define AR_MATCHING_WITHOUT_PCA 0 +// public static final int AR_MATCHING_WITH_PCA=1;//#define AR_MATCHING_WITH_PCA 1 + +// public static final int DEFAULT_TEMPLATE_MATCHING_MODE=AR_TEMPLATE_MATCHING_BW;//#define DEFAULT_TEMPLATE_MATCHING_MODE AR_TEMPLATE_MATCHING_COLOR +// public static final int DEFAULT_MATCHING_PCA_MODE=AR_MATCHING_WITH_PCA;//#define DEFAULT_MATCHING_PCA_MODE AR_MATCHING_WITHOUT_PCA + + + //#ifdef _WIN32 +// public static final int DEFAULT_IMAGE_PROC_MODE =AR_IMAGE_PROC_IN_FULL;//# define DEFAULT_IMAGE_PROC_MODE AR_IMAGE_PROC_IN_FULL +// public static final int DEFAULT_FITTING_MODE =AR_FITTING_TO_INPUT;//# define DEFAULT_FITTING_MODE AR_FITTING_TO_INPUT +// public static final int DEFAULT_DRAW_MODE =AR_DRAW_BY_TEXTURE_MAPPING;//# define DEFAULT_DRAW_MODE AR_DRAW_BY_TEXTURE_MAPPING +// public static final int DEFAULT_DRAW_TEXTURE_IMAGE =AR_DRAW_TEXTURE_FULL_IMAGE;//# define DEFAULT_DRAW_TEXTURE_IMAGE AR_DRAW_TEXTURE_FULL_IMAGE + //#endif + +// public static final int AR_PIX_SIZE_DEFAULT= +// (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ABGR) || (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGRA) || (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGBA) || (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_ARGB)?4: +// ((AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR) || (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB)?3: +// ((AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_2vuy) || (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_yuvs)?2: +// ((AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_MONO)?1:-1))); +// public static final int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5 +// public static final double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0 +// public static final double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR=1.0;//#define AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR 1.0 + +// public static final int AR_AREA_MAX=100000;//#define AR_AREA_MAX 100000 +// public static final int AR_AREA_MIN=70;//#define AR_AREA_MIN 70 + + +// public static final int AR_SQUARE_MAX=30;//#define AR_SQUARE_MAX 30 +// public static final int AR_CHAIN_MAX=10000;//#define AR_CHAIN_MAX 10000 +// public static final int AR_PATT_NUM_MAX=50;//#define AR_PATT_NUM_MAX 50 +// public static final int AR_PATT_SIZE_X=16;//#define AR_PATT_SIZE_X 16 +// public static final int AR_PATT_SIZE_Y=16;//#define AR_PATT_SIZE_Y 16 +// public static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 + +// public static final double AR_GL_CLIP_NEAR=50.0;//#define AR_GL_CLIP_NEAR 50.0 +// public static final double AR_GL_CLIP_FAR=5000.0;//#define AR_GL_CLIP_FAR 5000.0 + +// public static final int AR_HMD_XSIZE=640;//#define AR_HMD_XSIZE 640 +// public static final int AR_HMD_YSIZE=480;//#define AR_HMD_YSIZE 480 + +// public static final int AR_PARAM_NMIN=6;//#define AR_PARAM_NMIN 6 +// public static final int AR_PARAM_NMAX=1000;//#define AR_PARAM_NMAX 1000 +// public static final double AR_PARAM_C34=100.0;//#define AR_PARAM_C34 100.0 +} diff --git a/src/jp/nyatla/nyartoolkit/base/Param.java b/src/jp/nyatla/nyartoolkit/base/Param.java new file mode 100644 index 0000000..270589d --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/base/Param.java @@ -0,0 +1,366 @@ +package jp.nyatla.nyartoolkit.base; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.NyARMat; + + + + +public class Param { + private static final int AR_PARAM_NMIN=6;//#define AR_PARAM_NMIN 6 + private static final int AR_PARAM_NMAX=1000;//#define AR_PARAM_NMAX 1000 + private static final double AR_PARAM_C34=100.0;//#define AR_PARAM_C34 100.0 + + /* + typedef struct { + int xsize, ysize; + double matL[3][4]; + double matR[3][4]; + double matL2R[3][4]; + double dist_factorL[4]; + double dist_factorR[4]; + } ARSParam;*/ +/* static class ARSParam{ + int xsize, ysize; + Double2dArray matL=new Double2dArray(3,4); + Double2dArray matR=new Double2dArray(3,4); + Double2dArray matL2R=new Double2dArray(3,4); + double[] dist_factorL=new double[4]; + double[] dist_factorR=new double[4]; + }*/ + private static final int arParamGet_AR_PARAM_CDMIN = 12; + /*int arParamGet( double global[][3], double screen[][2], int data_num,double mat[3][4] );*/ + public static int arParamGet( double global[][], double[][] screen, int num,double[][] mat) throws NyARException + { + NyARMat mat_a, mat_at, mat_r;//ARMat *mat_a, *mat_at, *mat_r, mat_cpara; + NyARMat mat_wm1,mat_wm2;//ARMat *mat_wm1, *mat_wm2; + int i; + + if(num < AR_PARAM_NMIN){ + return( -1 ); + } + if(num > AR_PARAM_NMAX){ + return( -1 ); + } + NyARException.trap("未チェックのパス");{ + mat_a = new NyARMat(2*num,arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 ); + mat_at =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); + mat_r = new NyARMat(2*num,1);//mat_r = arMatrixAlloc( 2*num, 1 ); + mat_wm1 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 ); + mat_wm2 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); + } +/* + mat_a = Matrix.arMatrixAlloc( 2*num, arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 ); + if( mat_a == null){ + return -1; + } + mat_at =Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); + if(mat_at == null){ + return -1; + } + mat_r = Matrix.arMatrixAlloc( 2*num, 1 );//mat_r = arMatrixAlloc( 2*num, 1 ); + if(mat_r ==null){ + return -1; + } + mat_wm1 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 ); + if( mat_wm1 == null) { + return -1; + } + mat_wm2 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); + if( mat_wm2 == null ) { + return -1; + } +*/ + /* Initializing array */ + mat_a.zeroClear();//Javaではゼロクリアされるので不要 +// pa1=mat_a.getPointer();//pa1 = mat_a->m; +// for(i = 0; i < 2 * num * (arParamGet_AR_PARAM_CDMIN-1); i++){ +// {//*pa1++ = 0.0; +// pa1.set(0.0); +// pa1.incPtr();} +// } + double[][] pa1 =mat_a.getArray(); + double[][] pa2 =mat_a.getArray(); + /* Calculate A,R matrix */ + double[][] pr=mat_r.getArray(); + int pr_ptr_col=0; + for(i = 0; i < num; i++) {//for(i = 0, pr = mat_r->m; i < num; i++) { + int pa1_ptr_row =2*i; //pa1 = &(mat_a->m[ (2*i) * (AR_PARAM_CDMIN-1) + int pa2_ptr_row =2*i+1;//pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]); + + +// pa1=mat_a.getPointer((2*i)*(arParamGet_AR_PARAM_CDMIN-1)); ]); +// pa2=mat_a.getPointer((2*i+1)*(arParamGet_AR_PARAM_CDMIN-1) + 4); + //*pa1++ = global[i][0]; *pa1++ = global[i][1]; + pa1[pa1_ptr_row][ 0]=global[i][0]; + pa1[pa1_ptr_row][ 1]=global[i][1]; + //*pa1++ = global[i][2]; *pa1++ = 1.0; + pa1[pa1_ptr_row][ 2]=global[i][2]; + pa1[pa1_ptr_row][ 3]=1.0; + //*pa2++ = global[i][0]; *pa2++ = global[i][1]; + pa2[pa2_ptr_row][ 4]=global[i][0]; + pa2[pa2_ptr_row][ 5]=global[i][1]; + //*pa2++ = global[i][2]; *pa2++ = 1.0; + pa2[pa2_ptr_row][ 6]=global[i][2]; + pa2[pa2_ptr_row][ 7]=1.0; + //pa1 += 4; + //*pa1++ = -global[i][0] * screen[i][0]; + pa1[pa1_ptr_row][ 8]=-global[i][0] * screen[i][0]; + //*pa1++ = -global[i][1] * screen[i][0]; + pa1[pa1_ptr_row][ 9]=-global[i][1] * screen[i][0]; + //*pa1 = -global[i][2] * screen[i][0]; + pa1[pa1_ptr_row][10]=-global[i][2]* screen[i][0]; + //*pa2++ = -global[i][0] * screen[i][1]; + pa2[pa2_ptr_row][ 8]=-global[i][0] * screen[i][1]; + //*pa2++ = -global[i][1] * screen[i][1]; + pa2[pa2_ptr_row][ 9]=-global[i][1] * screen[i][1]; + //*pa2 = -global[i][2] * screen[i][1]; + pa2[pa2_ptr_row][10]=-global[i][2] * screen[i][1]; + //*pr++ = screen[i][0] * AR_PARAM_C34; + pr[0][pr_ptr_col]=screen[i][0] * AR_PARAM_C34;pr_ptr_col++; + //*pr++ = screen[i][1] * AR_PARAM_C34; + pr[0][pr_ptr_col]=screen[i][1] * AR_PARAM_C34;pr_ptr_col++; + } + + NyARException.trap("未チェックのパス"); + NyARMat.matrixTrans(mat_at, mat_a );//if( arMatrixTrans( mat_at, mat_a ) < 0 ){ + + NyARException.trap("未チェックのパス"); + mat_wm1.matrixMul(mat_at, mat_a );//if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) { + NyARException.trap("未チェックのパス"); + mat_wm1.matrixSelfInv();//if( arMatrixSelfInv( mat_wm1 ) < 0 ) { + + NyARException.trap("未チェックのパス"); + mat_wm2.matrixMul(mat_wm1, mat_at );//if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) { + + //mat_cpara.row = AR_PARAM_CDMIN-1;//mat_cpara.row = AR_PARAM_CDMIN-1; + //mat_cpara.clm = 1; + //mat_cpara.m = &(mat[0][0]); + /*1次元行列から3x4行列に転写。高負荷なところじゃないから地道に転写でOK*/ + NyARMat mat_cpara=new NyARMat(arParamGet_AR_PARAM_CDMIN-1,1); + double[][] mat_cpara_array=mat_cpara.getArray(); + NyARException.trap("未チェックのパス"); + mat_cpara.matrixMul(mat_wm2, mat_r );//if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) { + + for(int i2=0;i<3;i++){ + for(int i3=0;i3<4;i3++){ + mat[i2][i3]=mat_cpara_array[i2*4+i3][0]; + } + } + //ARMat.wrap(mat.array(),arParamGet_AR_PARAM_CDMIN-1,1);} + + mat[2][3]=AR_PARAM_C34;//mat[2][3] = AR_PARAM_C34; + + return 0; + } + +/* //int arsParamChangeSize( ARSParam *source, int xsize, int ysize, ARSParam *newparam ); + public static int arsParamChangeSize( ARSParam source, int xsize, int ysize, ARSParam newparam) + { + double scale; + + newparam.xsize=xsize;//newparam->xsize = xsize; + newparam.ysize=ysize;//newparam->ysize = ysize; + + scale=(double)xsize/ (double)(source.xsize);//scale = (double)xsize / (double)(source->xsize); + for(int i = 0; i < 4; i++ ){ + newparam.matL.set(0,i,source.matL.get(0,i)*scale);//newparam->matL[0][i] = source->matL[0][i] * scale; + newparam.matL.set(1,i,source.matL.get(1,i)*scale);//newparam->matL[1][i] = source->matL[1][i] * scale; + newparam.matL.set(2,i,source.matL.get(2,i));//newparam->matL[2][i] = source->matL[2][i]; + } + for(int i = 0; i < 4; i++ ) { + newparam.matR.set(0,i,source.matR.get(0,i)*scale);//newparam->matR[0][i] = source->matR[0][i] * scale; + newparam.matR.set(1,i,source.matR.get(1,i)*scale);//newparam->matR[1][i] = source->matR[1][i] * scale; + newparam.matR.set(2,i,source.matR.get(2,i));//newparam->matR[2][i] = source->matR[2][i]; + } + for(int i = 0; i < 4; i++ ) { + newparam.matL2R.set(0,i,source.matL2R.get(0,i));//newparam->matL2R[0][i] = source->matL2R[0][i]; + newparam.matL2R.set(1,i,source.matL2R.get(1,i));//newparam->matL2R[1][i] = source->matL2R[1][i]; + newparam.matL2R.set(2,i,source.matL2R.get(2,i));//newparam->matL2R[2][i] = source->matL2R[2][i]; + } + + newparam.dist_factorL[0] = source.dist_factorL[0] * scale;//newparam->dist_factorL[0] = source->dist_factorL[0] * scale; + newparam.dist_factorL[1] = source.dist_factorL[1] * scale;//newparam->dist_factorL[1] = source->dist_factorL[1] * scale; + newparam.dist_factorL[2] = source.dist_factorL[2] / (scale*scale);//newparam->dist_factorL[2] = source->dist_factorL[2] / (scale*scale); + newparam.dist_factorL[3] = source.dist_factorL[3];//newparam->dist_factorL[3] = source->dist_factorL[3]; + + newparam.dist_factorR[0] = source.dist_factorR[0] * scale;//newparam->dist_factorR[0] = source->dist_factorR[0] * scale; + newparam.dist_factorR[1] = source.dist_factorR[1] * scale;//newparam->dist_factorR[1] = source->dist_factorR[1] * scale; + newparam.dist_factorR[2] = source.dist_factorR[2] / (scale*scale);//newparam->dist_factorR[2] = source->dist_factorR[2] / (scale*scale); + newparam.dist_factorR[3] = source.dist_factorR[3];//newparam->dist_factorR[3] = source->dist_factorR[3]; + + return 0; + }*/ +/* //int arsParamSave( char *filename, ARSParam *sparam ); + public static int arsParamSave(String filename, ARSParam sparam) throws Exception + { + //int xsize, ysize; + //Double2dArray matL=new Double2dArray(3,4); + //Double2dArray matR=new Double2dArray(3,4); + //Double2dArray matL2R=new Double2dArray(3,4); + //double dist_factorL[]=new double[4]; + //double dist_factorR[]=new double[4]; + + byte[] buf=new byte[(4+4+(3*4*8)*3+(4*8)*2)]; + + //バッファをラップ + ByteBuffer bb = ByteBuffer.wrap(buf); + bb.order(ByteOrder.BIG_ENDIAN); + + //書き込み + bb.putInt(sparam.xsize); + bb.putInt(sparam.ysize); + for(int i=0;i<3;i++){ + for(int i2=0;i2<4;i2++){ + bb.putDouble(sparam.matL.get(i, i2)); + } + } + for(int i=0;i<3;i++){ + for(int i2=0;i2<4;i2++){ + bb.putDouble(sparam.matR.get(i, i2)); + } + } + for(int i=0;i<3;i++){ + for(int i2=0;i2<4;i2++){ + bb.putDouble(sparam.matL2R.get(i, i2)); + } + } + for(int i=0;i<4;i++){ + bb.putDouble(sparam.dist_factorL[i]); + } + for(int i=0;i<4;i++){ + bb.putDouble(sparam.dist_factorR[i]); + } + //ファイルに保存 + FileOutputStream fs=new FileOutputStream(filename); + fs.write(buf); + fs.close(); + + return 0; + }*/ +/* //int arsParamLoad( char *filename, ARSParam *sparam ); + public static int arsParamLoad(String filename, ARSParam sparam ) throws Exception + { + //ファイルを読んどく + FileInputStream fs=new FileInputStream(filename); + File f=new File(filename); + int file_size=(int)f.length(); + byte[] buf=new byte[file_size]; + fs.read(buf); + fs.close(); + + //バッファを加工 + ByteBuffer bb = ByteBuffer.wrap(buf); + bb.order(ByteOrder.BIG_ENDIAN); + + //固定回数パースして配列に格納 + sparam.xsize=bb.getInt(); + sparam.ysize=bb.getInt(); + for(int i=0;i<3;i++){ + for(int i2=0;i2<4;i2++){ + sparam.matL.set(i,i2,bb.getDouble()); + } + } + for(int i=0;i<3;i++){ + for(int i2=0;i2<4;i2++){ + sparam.matR.set(i,i2,bb.getDouble()); + } + } + for(int i=0;i<3;i++){ + for(int i2=0;i2<4;i2++){ + sparam.matL2R.set(i,i2,bb.getDouble()); + } + } + for(int i=0;i<3;i++){ + sparam.dist_factorL[i]=bb.getDouble(); + } + for(int i=0;i<3;i++){ + sparam.dist_factorR[i]=bb.getDouble(); + } + return 0; + }*/ +/* + // int arsParamGetMat( double matL[3][4], double matR[3][4],double cparaL[3][4], double cparaR[3][4], double matL2R[3][4] ); + public static int arsParamGetMat(double[][] matL, double[][] matR,double[][] cparaL, double[][] cparaR, double[][] matL2R) throws JartkException + { + ARMat t1,t2,t3;//ARMat *t1, *t2, *t3; + //double transL[3][4], transR[3][4]; + Double2dArray transL=new Double2dArray(3,4); + Double2dArray transR=new Double2dArray(3,4); + + arParamDecompMat( matL,cparaL,transL); + arParamDecompMat( matR,cparaR,transR); + JartkException.trap("未チェックパス");{ + t1=new ARMat(4,4);//t1 = arMatrixAlloc( 4, 4 ); + t2=new ARMat(4,4);//t2 = arMatrixAlloc( 4, 4 ); + } + double[][] t1_array=t1.getArray(); + double[][] t2_array=t2.getArray(); + for(int j = 0; j < 3; j++ ){ + for(int i = 0; i < 4; i++ ) { + JartkException.trap("未チェックパス");{ + t1_array[j][i]=transL.get(j,i);//t1->m[j*4+i] = transL[j][i]; + t2_array[j][i]=transL.get(j,i);//t2->m[j*4+i] = transR[j][i]; + } + } + } + JartkException.trap("未チェックパス");{ + t1_array[3][0]=t1_array[3][1]=t1_array[3][2]=0.0;//t1->m[12] = t1->m[13] = t1->m[14] = 0.0; + t1_array[3][3]=1.0;//t1->m[15] = 1.0; + t2_array[3][0]=t2_array[3][1]=t2_array[3][2]=0.0;//t2->m[12] = t2->m[13] = t2->m[14] = 0.0; + t2_array[3][3]=1.0;//t2->m[15] = 1.0; + } + JartkException.trap("未チェックのパス"); + t1.matrixSelfInv();//if( arMatrixSelfInv(t1) != 0 ) { + + JartkException.trap("未チェックのパス"); + t3 =ARMat.matrixAllocMul(t2, t1);//t3 = arMatrixAllocMul(t2, t1); + double[][] t3_array=t3.getArray(); + if(t3==null){ + return -1; + } + + for(int j = 0; j < 3; j++ ) { + for(int i = 0; i < 4; i++ ) { + JartkException.trap("未チェックパス"); + matL2R[j][i]=t3_array[j][i];//matL2R[j][i] = t3->m[j*4+i]; + } + } + return 0; + } +*/ //int arsParamDisp( ARSParam *sparam ) +/* public static int arsParamDisp( ARSParam sparam) + { + System.out.println("--------------------------------------");//printf("--------------------------------------\n"); + System.out.println("SIZE = "+sparam.xsize+", "+sparam.ysize);// printf("SIZE = %d, %d\n", sparam->xsize, sparam->ysize); + System.out.println("-- Left --");//printf("-- Left --\n"); + System.out.println("Distotion factor = "+sparam.dist_factorL[0]+" "+sparam.dist_factorL[1]+" "+sparam.dist_factorL[2]+" "+sparam.dist_factorL[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorL[0],sparam->dist_factorL[1], sparam->dist_factorL[2], sparam->dist_factorL[3] ); + for(int j = 0; j < 3; j++ ) { + for(int i = 0; i < 4; i++ ){ + System.out.print(sparam.matL.get(j,i)+" ");//printf("%7.5f ", sparam->matL[j][i]); + } + System.out.println();//printf("\n"); + } + + System.out.println("-- Right --");//printf("-- Right --\n"); + System.out.println("Distotion factor = "+sparam.dist_factorR[0]+" "+sparam.dist_factorR[1]+" "+sparam.dist_factorR[2]+" "+sparam.dist_factorR[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorR[0],sparam->dist_factorR[1], sparam->dist_factorR[2], sparam->dist_factorR[3]); + for(int j = 0; j < 3; j++ ){ + for(int i = 0; i < 4; i++ ){ + System.out.println(sparam.matR.get(j,i)+" ");//printf("%7.5f ", sparam->matR[j][i]); + } + System.out.println();//printf("\n"); + } + + System.out.println("-- Left => Right --");//printf("-- Left => Right --\n"); + for(int j = 0; j < 3; j++ ) { + for(int i = 0; i < 4; i++ ){ + //printf("%7.5f ", sparam->matL2R[j][i]); + } + System.out.println();//printf("\n"); + } + + System.out.println("--------------------------------------");//printf("--------------------------------------\n"); + + return 0; + }*/ +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARCode.java b/src/jp/nyatla/nyartoolkit/core/NyARCode.java new file mode 100644 index 0000000..9f067ab --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARCode.java @@ -0,0 +1,180 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ + +package jp.nyatla.nyartoolkit.core; + +import java.io.FileInputStream; +import java.io.InputStream; +import java.io.InputStreamReader; +import java.io.StreamTokenizer; + +import jp.nyatla.nyartoolkit.NyARException; + + + + + +/** + * ARToolKitのマーカーコードを1個保持します。 + * + */ +public class NyARCode{ + private int[][][][] pat;//static int pat[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3]; + private double[] patpow=new double[4];//static double patpow[AR_PATT_NUM_MAX][4]; + private short[][][] patBW;//static int patBW[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3]; + private double[] patpowBW=new double[4];//static double patpowBW[AR_PATT_NUM_MAX][4]; + private int width,height; + public int[][][][] getPat() + { + return pat; + } + public double [] getPatPow() + { + return patpow; + } + public short[][][] getPatBW() + { + return patBW; + } + public double[] getPatPowBW() + { + return patpowBW; + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + + public NyARCode(int i_width,int i_height) + { + width=i_width; + height=i_height; + pat=new int[4][height][width][3];//static int pat[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3]; + patBW=new short[4][height][width];//static int patBW[AR_PATT_NUM_MAX][4][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3]; + } + /** + * int arLoadPatt( const char *filename ); + * ARToolKitのパターンファイルをロードする。 + * ファイル形式はBGR形式で記録されたパターンファイルであること。 + * @param filename + * @return + * @throws Exception + */ + public void loadFromARFile(String filename) throws NyARException + { + try { + loadFromARFile(new FileInputStream(filename)); + + } catch (Exception e) { + throw new NyARException(e); + } + } + /** + * + * @param i_stream + * @throws NyARException + */ + public void loadFromARFile(InputStream i_stream) throws NyARException + { + try{ + StreamTokenizer st=new StreamTokenizer(new InputStreamReader(i_stream)); + //パターンデータはGBRAで並んでる。 + for(int h=0; h<4; h++ ) { + int l = 0; + for(int i3 = 0; i3 < 3; i3++ ) { + for(int i2 = 0; i2 < height; i2++ ) { + for(int i1 = 0; i1 < width; i1++ ){ + //数値のみ読み出す + switch(st.nextToken()){//if( fscanf(fp, "%d", &j) != 1 ) { + case StreamTokenizer.TT_NUMBER: + break; + default: + throw new NyARException(); + } + short j=(short)(255-st.nval);//j = 255-j; + //標準ファイルのパターンはBGRでならんでるからRGBに並べなおす + switch(i3){ + case 0:pat[h][i2][i1][2] = j;break;//pat[patno][h][(i2*Config.AR_PATT_SIZE_X+i1)*3+2] = j;break; + case 1:pat[h][i2][i1][1] = j;break;//pat[patno][h][(i2*Config.AR_PATT_SIZE_X+i1)*3+1] = j;break; + case 2:pat[h][i2][i1][0] = j;break;//pat[patno][h][(i2*Config.AR_PATT_SIZE_X+i1)*3+0] = j;break; + } + //pat[patno][h][(i2*Config.AR_PATT_SIZE_X+i1)*3+i3] = j; + if( i3 == 0 ){ + patBW[h][i2][i1] = j;//patBW[patno][h][i2*Config.AR_PATT_SIZE_X+i1] = j; + }else{ + patBW[h][i2][i1] += j;//patBW[patno][h][i2*Config.AR_PATT_SIZE_X+i1] += j; + } + if( i3 == 2 ){ + patBW[h][i2][i1] /= 3;//patBW[patno][h][i2*Config.AR_PATT_SIZE_X+i1] /= 3; + } + l += j; + } + } + } + + l /= (height*width*3); + + int m = 0; + for(int i = 0; i < height; i++ ) {//for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; i++ ) { + for(int i2 = 0; i2 < width; i2++ ) { + for(int i3 = 0; i3 < 3; i3++ ) { + pat[h][i][i2][i3] -= l; + m += (pat[h][i][i2][i3]*pat[h][i][i2][i3]); + } + } + } + patpow[h] = Math.sqrt((double)m); + if( patpow[h] == 0.0 ){ + patpow[h] = 0.0000001; + } + + m = 0; + for(int i = 0; i < height; i++ ) { + for(int i2 = 0; i2 < width; i2++ ) { + patBW[h][i][i2] -= l; + m += (patBW[h][i][i2]*patBW[h][i][i2]); + } + } + patpowBW[h] = Math.sqrt((double)m); + if(patpowBW[h] == 0.0 ){ + patpowBW[h] = 0.0000001; + } + } + }catch(Exception e){ + throw new NyARException(e); + } + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java new file mode 100644 index 0000000..32143de --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt.java @@ -0,0 +1,76 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.*; + +public interface NyARColorPatt +{ +// 消すかも。 +// /** +// * カラーパターンのサイズを変更します。 +// * 変更を行うと、既にgetPatArrayで参照中の配列内容は不定になり、インスタンスのパラメータは初期状態に戻ります。 +// * @param i_new_width +// * 新しいパターン幅 +// * @param i_new_height +// * 新しいパターン高 +// */ +// public void setSize(int i_new_width,int i_new_height); + /** + * カラーパターンの幅をピクセル値で返します。 + * @return + */ + public int getWidth(); + /** + * カラーパターンの高さをピクセル値で返します。 + * @return + */ + public int getHeight(); + /** + * カメラパターンを格納した配列への参照値を返します。 + * 配列は最低でも[height][wight][3]のサイズを持ちますが、 + * 配列のlengthとwidth,heightの数は一致しないことがあります。 + * setSize関数を実行すると、以前に呼び出されたgetPatArrayが返した値は不定になります。 + * @return + */ + public int[][][] getPatArray(); + /** + * ラスタイメージからi_marker部分のカラーパターンを抽出して、保持します。 + * @param image + * @param i_marker + * @return + * ラスターの取得に成功するとTRUE/失敗するとFALSE + * @throws NyARException + */ + public boolean pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException; +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O1.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O1.java new file mode 100644 index 0000000..0800cb1 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O1.java @@ -0,0 +1,324 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.NyARRaster; + +/** + * 24ビットカラーのマーカーを保持するために使うクラスです。 + * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 + * 演算順序以外の最適化をしたもの + * + */ +public class NyARColorPatt_O1 implements NyARColorPatt +{ + private static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 + private int extpat[][][]; + private int width; + private int height; + public NyARColorPatt_O1(int i_width,int i_height) + { + this.width =i_width; + this.height=i_height; + this.extpat=new int[i_height][i_width][3]; + this.wk_pickFromRaster_ext_pat2=new int[i_height][i_width][3]; + } +// public void setSize(int i_new_width,int i_new_height) +// { +// int array_w=this.extpat[0].length; +// int array_h=this.extpat.length; +// //十分なサイズのバッファがあるか確認 +// if(array_w>=i_new_width && array_h>=i_new_height){ +// //OK 十分だ→サイズ調整のみ +// }else{ +// //足りないよ→取り直し +// this.wk_pickFromRaster_ext_pat2=new int[i_new_height][i_new_width][3]; +// this.extpat=new int[i_new_height][i_new_width][3]; +// } +// this.width =i_new_width; +// this.height=i_new_height; +// return; +// } + + public int[][][] getPatArray() + { + return extpat; + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + private final NyARMat wk_get_cpara_a=new NyARMat(8,8); + private final NyARMat wk_get_cpara_b=new NyARMat(8,1); + private final NyARMat wk_get_cpara_c=new NyARMat(8,1); + /** + * + * @param world + * @param vertex + * @param para + * [3x3] + * @throws NyARException + */ + private boolean get_cpara( double world[][], double vertex[][],double[] para) throws NyARException + { + NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 ); + double[][] a_array=a.getArray(); + NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 ); + double[][] b_array=b.getArray(); + double[] a_pt0,a_pt1,world_pti; + + for(int i = 0; i < 4; i++ ) { + a_pt0=a_array[i*2]; + a_pt1=a_array[i*2+1]; + world_pti=world[i]; + + a_pt0[0]=world_pti[0];//a->m[i*16+0] = world[i][0]; + a_pt0[1]=world_pti[1];//a->m[i*16+1] = world[i][1]; + a_pt0[2]=1.0;//a->m[i*16+2] = 1.0; + a_pt0[3]=0.0;//a->m[i*16+3] = 0.0; + a_pt0[4]=0.0;//a->m[i*16+4] = 0.0; + a_pt0[5]=0.0;//a->m[i*16+5] = 0.0; + a_pt0[6]=-world_pti[0] * vertex[i][0];//a->m[i*16+6] = -world[i][0] * vertex[i][0]; + a_pt0[7]=-world_pti[1] * vertex[i][0];//a->m[i*16+7] = -world[i][1] * vertex[i][0]; + a_pt1[0]=0.0;//a->m[i*16+8] = 0.0; + a_pt1[1]=0.0;//a->m[i*16+9] = 0.0; + a_pt1[2]=0.0;//a->m[i*16+10] = 0.0; + a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0]; + a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1]; + a_pt1[5]=1.0;//a->m[i*16+13] = 1.0; + a_pt1[6]=-world_pti[0] * vertex[i][1];//a->m[i*16+14] = -world[i][0] * vertex[i][1]; + a_pt1[7]=-world_pti[1] * vertex[i][1];//a->m[i*16+15] = -world[i][1] * vertex[i][1]; + b_array[i*2+0][0]=vertex[i][0];//b->m[i*2+0] = vertex[i][0]; + b_array[i*2+1][0]=vertex[i][1];//b->m[i*2+1] = vertex[i][1]; + } +// JartkException.trap("未チェックのパス"); + if(!a.matrixSelfInv()){ + return false;//逆行列を求められないので失敗 + } + +// JartkException.trap("未チェックのパス"); + NyARMat c = wk_get_cpara_c;//次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 ); + double[][] c_array=c.getArray(); + + c.matrixMul(a, b); + for(int i = 0; i < 2; i++ ) { + para[i*3+0] = c_array[i*3+0][0];//para[i][0] = c->m[i*3+0]; + para[i*3+1] = c_array[i*3+1][0];//para[i][1] = c->m[i*3+1]; + para[i*3+2] = c_array[i*3+2][0];//para[i][2] = c->m[i*3+2]; + } + para[2*3+0] = c_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; + para[2*3+1] = c_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; + para[2*3+2] = 1.0;//para[2][2] = 1.0; + return true; + } + + private final double[][] wk_pickFromRaster_local=new double[4][2]; + private final double[] wk_pickFromRaster_para=new double[9];//[3][3]; + private int[][][] wk_pickFromRaster_ext_pat2=null;//コンストラクタでint[height][width][3]を作る + private final double[][] wk_pickFromRaster_world={//double world[4][2]; + {100.0, 100.0}, + {100.0+10.0,100.0}, + {100.0+10.0,100.0 + 10.0}, + {100.0, 100.0 + 10.0} + }; + /** + * pickFromRaster関数から使う変数です。 + * + */ + private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height) + { + int i,i2; + int[][] pt2; + int[] pt1; + for(i=i_height-1;i>=0;i--){ + pt2=i_ext_pat2[i]; + for(i2=i_width-1;i2>=0;i2--){ + pt1=pt2[i2]; + pt1[0]=0; + pt1[1]=0; + pt1[2]=0; + } + } + } + private final int[] wk_pickFromRaster_rgb_tmp=new int[3]; + /** + * imageから、i_markerの位置にあるパターンを切り出して、保持します。 + * Optimize:STEP[769->] + * @param image + * @param i_marker + * @return + * 切り出しに失敗した + * @throws Exception + */ + public boolean pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException + { + double d, xw, yw; + int xc, yc; + int xdiv, ydiv; + int xdiv2, ydiv2; + int lx1, lx2, ly1, ly2; + + int img_x=image.getWidth(); + int img_y=image.getHeight(); + + double xdiv2_reciprocal; // [tp] + double ydiv2_reciprocal; // [tp] + + + int[] x_coord=i_marker.x_coord; + int[] y_coord=i_marker.y_coord; + double[][] local=wk_pickFromRaster_local;//double local[4][2]; + int[] vertex=i_marker.mkvertex; + for(int i = 0; i < 4; i++ ) { + local[i][0] = x_coord[vertex[i]]; + local[i][1] = y_coord[vertex[i]]; + } + double[][] world=wk_pickFromRaster_world; +/* world[0][0] = 100.0; + world[0][1] = 100.0; + world[1][0] = 100.0 + 10.0; + world[1][1] = 100.0; + world[2][0] = 100.0 + 10.0; + world[2][1] = 100.0 + 10.0; + world[3][0] = 100.0; + world[3][1] = 100.0 + 10.0;*/ + double[] para =wk_pickFromRaster_para; //double para[3][3]; + //パターンの切り出しに失敗することもある。 + if(!get_cpara( world, local, para )){ + return false; + } + lx1 = (int)((local[0][0] - local[1][0])*(local[0][0] - local[1][0])+ (local[0][1] - local[1][1])*(local[0][1] - local[1][1])); + lx2 = (int)((local[2][0] - local[3][0])*(local[2][0] - local[3][0])+ (local[2][1] - local[3][1])*(local[2][1] - local[3][1])); + ly1 = (int)((local[1][0] - local[2][0])*(local[1][0] - local[2][0])+ (local[1][1] - local[2][1])*(local[1][1] - local[2][1])); + ly2 = (int)((local[3][0] - local[0][0])*(local[3][0] - local[0][0])+ (local[3][1] - local[0][1])*(local[3][1] - local[0][1])); + if( lx2 > lx1 ){ + lx1 = lx2; + } + if( ly2 > ly1 ){ + ly1 = ly2; + } + xdiv2 =this.width; + ydiv2 =this.height; + + while( xdiv2*xdiv2 < lx1/4 ){ + xdiv2*=2; + } + while( ydiv2*ydiv2 < ly1/4 ){ + ydiv2*=2; + } + + if( xdiv2 > AR_PATT_SAMPLE_NUM) + { + xdiv2 =AR_PATT_SAMPLE_NUM; + } + if( ydiv2 >AR_PATT_SAMPLE_NUM) + { + ydiv2 = AR_PATT_SAMPLE_NUM; + } + + xdiv = xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X; + ydiv = ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y; + + /*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/ + int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; + int extpat_j[][],extpat_j_i[]; + int ext_pat2_j[][],ext_pat2_j_i[]; + + initValue_wk_pickFromRaster_ext_pat2(ext_pat2,this.width,this.height); + + xdiv2_reciprocal = 1.0 / xdiv2; + ydiv2_reciprocal = 1.0 / ydiv2; + int i,j; + int[] rgb_tmp=wk_pickFromRaster_rgb_tmp; + // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) ); + for(j = 0; j < ydiv2; j++ ) { + yw = 102.5 + 5.0 * (j+0.5) * ydiv2_reciprocal; + for(i = 0; i < xdiv2; i++ ) { + xw = 102.5 + 5.0 * (i+0.5) * xdiv2_reciprocal; + d = para[2*3+0]*xw + para[2*3+1]*yw + para[2*3+2]; + if( d == 0 ){ + throw new NyARException(); + } + xc = (int)((para[0*3+0]*xw + para[0*3+1]*yw + para[0*3+2])/d); + yc = (int)((para[1*3+0]*xw + para[1*3+1]*yw + para[1*3+2])/d); + + + if( xc >= 0 && xc < img_x && yc >= 0 && yc < img_y ) { + image.getPixel(xc, yc, rgb_tmp); + ext_pat2_j_i=ext_pat2[j/ydiv][i/xdiv]; + + ext_pat2_j_i[0] += rgb_tmp[0];//R + ext_pat2_j_i[1] += rgb_tmp[1];//G + ext_pat2_j_i[2] += rgb_tmp[2];//B +// System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]); + } + } + } +// short[][][] ext_pat=new short[Config.AR_PATT_SIZE_Y][Config.AR_PATT_SIZE_X][3];//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; + /**/ + int xdiv_x_ydiv=xdiv*ydiv; + for(j = this.height-1; j>=0; j--){ + extpat_j=extpat[j]; + ext_pat2_j=ext_pat2[j]; + for(i = this.width-1; i>=0; i--){ // PRL 2006-06-08. + ext_pat2_j_i=ext_pat2_j[i]; + extpat_j_i=extpat_j[i]; + extpat_j_i[0]=(ext_pat2_j_i[0] / xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_j_i[1]=(ext_pat2_j_i[1] / xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_j_i[2]=(ext_pat2_j_i[2] / xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + }/* + int xdiv_mul_ydiv=xdiv*ydiv; + short [][] extpat_pt_2; + short[] extpat_pt_1; + int[][] ext_pat2_pt_2; + int[] ext_pat2_pt_1; + for(int j=this.height-1; j>=0; j--){//for(int j = 0; j < this.height; j++ ){ + extpat_pt_2=extpat[j]; + ext_pat2_pt_2=ext_pat2[j]; + for(int i = this.width-1; i>=0; i--){//for(int i = 0; i < this.width; i++ ){ // PRL 2006-06-08. + extpat_pt_1=extpat_pt_2[i]; + ext_pat2_pt_1=ext_pat2_pt_2[i]; + extpat_pt_1[0]=(short)(ext_pat2_pt_1[0] / xdiv_mul_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_pt_1[1]=(short)(ext_pat2_pt_1[1] / xdiv_mul_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_pt_1[2]=(short)(ext_pat2_pt_1[2] / xdiv_mul_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + } + /**/ + return true; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java new file mode 100644 index 0000000..ca2dcc2 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O2.java @@ -0,0 +1,323 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.NyARRaster; + +/** + * 24ビットカラーのマーカーを保持するために使うクラスです。 + * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 + * 演算順序を含む最適化をしたもの + * + */ +public class NyARColorPatt_O2 implements NyARColorPatt +{ + private static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 + private int extpat[][][]; + private int width; + private int height; + public NyARColorPatt_O2(int i_width,int i_height) + { + this.width=i_width; + this.height=i_height; + this.extpat=new int[i_height][i_width][3]; + this.wk_pickFromRaster_ext_pat2=new int[i_height][i_width][3]; + } +// public void setSize(int i_new_width,int i_new_height) +// { +// int array_w=this.extpat[0].length; +// int array_h=this.extpat.length; +// //十分なサイズのバッファがあるか確認 +// if(array_w>=i_new_width && array_h>=i_new_height){ +// //OK 十分だ→サイズ調整のみ +// }else{ +// //足りないよ→取り直し +// this.wk_pickFromRaster_ext_pat2=new int[i_new_height][i_new_width][3]; +// this.extpat=new int[i_new_height][i_new_width][3]; +// } +// this.width =i_new_width; +// this.height=i_new_height; +// return; +// } + public int[][][] getPatArray() + { + return extpat; + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + private final NyARMat wk_get_cpara_a=new NyARMat(8,8); + private final NyARMat wk_get_cpara_b=new NyARMat(8,1); +// private final NyARMat wk_get_cpara_c=new NyARMat(8,1); + /** + * @param world + * @param vertex + * @param o_para + * @throws NyARException + */ + private boolean get_cpara(double vertex_0[], double vertex_1[],NyARMat o_para) throws NyARException + { + double world[][]=this.wk_pickFromRaster_world; + NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 ); + double[][] a_array=a.getArray(); + NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 ); + double[][] b_array=b.getArray(); + double[] a_pt0,a_pt1,world_pti; + + for(int i = 0; i < 4; i++ ) { + a_pt0=a_array[i*2]; + a_pt1=a_array[i*2+1]; + world_pti=world[i]; + + a_pt0[0]=world_pti[0];//a->m[i*16+0] = world[i][0]; + a_pt0[1]=world_pti[1];//a->m[i*16+1] = world[i][1]; + a_pt0[2]=1.0;//a->m[i*16+2] = 1.0; + a_pt0[3]=0.0;//a->m[i*16+3] = 0.0; + a_pt0[4]=0.0;//a->m[i*16+4] = 0.0; + a_pt0[5]=0.0;//a->m[i*16+5] = 0.0; + a_pt0[6]=-world_pti[0] * vertex_0[i];//a->m[i*16+6] = -world[i][0] * vertex[i][0]; + a_pt0[7]=-world_pti[1] * vertex_0[i];//a->m[i*16+7] = -world[i][1] * vertex[i][0]; + a_pt1[0]=0.0;//a->m[i*16+8] = 0.0; + a_pt1[1]=0.0;//a->m[i*16+9] = 0.0; + a_pt1[2]=0.0;//a->m[i*16+10] = 0.0; + a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0]; + a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1]; + a_pt1[5]=1.0;//a->m[i*16+13] = 1.0; + a_pt1[6]=-world_pti[0] * vertex_1[i];//a->m[i*16+14] = -world[i][0] * vertex[i][1]; + a_pt1[7]=-world_pti[1] * vertex_1[i];//a->m[i*16+15] = -world[i][1] * vertex[i][1]; + b_array[i*2+0][0]=vertex_0[i];//b->m[i*2+0] = vertex[i][0]; + b_array[i*2+1][0]=vertex_1[i];//b->m[i*2+1] = vertex[i][1]; + } +// JartkException.trap("未チェックのパス"); + if(!a.matrixSelfInv()){ + return false; + } + +// JartkException.trap("未チェックのパス"); +// NyARMat c = wk_get_cpara_c;//次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 ); +// double[][] c_array=c.getArray(); + + o_para.matrixMul(a, b); +// para[0*3+0] = c_array[0*3+0][0];//para[i][0] = c->m[i*3+0]; +// para[0*3+1] = c_array[0*3+1][0];//para[i][1] = c->m[i*3+1]; +// para[0*3+2] = c_array[0*3+2][0];//para[i][2] = c->m[i*3+2]; +// para[1*3+0] = c_array[1*3+0][0];//para[i][0] = c->m[i*3+0]; +// para[1*3+1] = c_array[1*3+1][0];//para[i][1] = c->m[i*3+1]; +// para[i*3+2] = c_array[1*3+2][0];//para[i][2] = c->m[i*3+2]; +// para[2*3+0] = c_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; +// para[2*3+1] = c_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; +// para[2*3+2] = 1.0;//para[2][2] = 1.0; + return true; + } + + // private final double[] wk_pickFromRaster_para=new double[9];//[3][3]; + private int[][][] wk_pickFromRaster_ext_pat2=null;//コンストラクタでint[height][width][3]を作る + private final double[][] wk_pickFromRaster_world={//double world[4][2]; + {100.0, 100.0}, + {100.0+10.0,100.0}, + {100.0+10.0,100.0 + 10.0}, + {100.0, 100.0 + 10.0} + }; + /** + * pickFromRaster関数から使う変数です。 + * + */ + private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height) + { + int i,i2; + int[][] pt2; + int[] pt1; + for(i=i_height-1;i>=0;i--){ + pt2=i_ext_pat2[i]; + for(i2=i_width-1;i2>=0;i2--){ + pt1=pt2[i2]; + pt1[0]=0; + pt1[1]=0; + pt1[2]=0; + } + } + } + private final double[][] wk_pickFromRaster_local=new double[2][4]; + private final int[] wk_pickFromRaster_rgb_tmp=new int[3]; + private final NyARMat wk_pickFromRaster_cpara=new NyARMat(8,1); + /** + * imageから、i_markerの位置にあるパターンを切り出して、保持します。 + * Optimize:STEP[769->750] + * @param image + * @param i_marker + * @throws Exception + */ + public boolean pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException + { + NyARMat cpara=this.wk_pickFromRaster_cpara; + //localの計算 + int[] x_coord=i_marker.x_coord; + int[] y_coord=i_marker.y_coord; + int[] vertex=i_marker.mkvertex; + double[] local_0=wk_pickFromRaster_local[0];//double local[4][2]; + double[] local_1=wk_pickFromRaster_local[1];//double local[4][2]; + for(int i = 0; i < 4; i++ ) { + local_0[i] = x_coord[vertex[i]]; + local_1[i] = y_coord[vertex[i]]; + } + //xdiv2,ydiv2の計算 + int xdiv2, ydiv2; + int l1,l2; + double w1,w2; + + //x計算 + w1=local_0[0] - local_0[1]; + w2=local_1[0] - local_1[1]; + l1 = (int)(w1*w1+w2*w2); + w1=local_0[2] - local_0[3]; + w2=local_1[2] - local_1[3]; + l2 = (int)(w1*w1+w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + l1=l1/4; + xdiv2 =this.width; + while( xdiv2*xdiv2 < l1 ){ + xdiv2*=2; + } + if( xdiv2 > AR_PATT_SAMPLE_NUM) + { + xdiv2 =AR_PATT_SAMPLE_NUM; + } + + //y計算 + w1=local_0[1] - local_0[2]; + w2=local_1[1] - local_1[2]; + l1 = (int)(w1*w1+ w2*w2); + w1=local_0[3] - local_0[0]; + w2=local_1[3] - local_1[0]; + l2 = (int)(w1*w1+ w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + ydiv2 =this.height; + l1=l1/4; + while( ydiv2*ydiv2 < l1 ){ + ydiv2*=2; + } + if( ydiv2 >AR_PATT_SAMPLE_NUM) + { + ydiv2 = AR_PATT_SAMPLE_NUM; + } + + //cparaの計算 + if(!get_cpara(local_0,local_1,cpara)){ + return false; + } + + int img_x=image.getWidth(); + int img_y=image.getHeight(); + + /*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/ + int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3]; + int extpat_j[][],extpat_j_i[]; + int ext_pat2_j[][],ext_pat2_j_i[]; + + initValue_wk_pickFromRaster_ext_pat2(ext_pat2,this.width,this.height); + + double[][] cpara_array=cpara.getArray(); + double para21_x_yw,para01_x_yw,para11_x_yw; + double para00,para01,para02,para10,para11,para12,para20,para21; + para00 = cpara_array[0*3+0][0];//para[i][0] = c->m[i*3+0]; + para01 = cpara_array[0*3+1][0];//para[i][1] = c->m[i*3+1]; + para02 = cpara_array[0*3+2][0];//para[i][2] = c->m[i*3+2]; + para10 = cpara_array[1*3+0][0];//para[i][0] = c->m[i*3+0]; + para11 = cpara_array[1*3+1][0];//para[i][1] = c->m[i*3+1]; + para12 = cpara_array[1*3+2][0];//para[i][2] = c->m[i*3+2]; + para20 = cpara_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; + para21 = cpara_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; + //para22 = 1.0;//para[2][2] = 1.0; + + + double d, xw, yw; + int xc, yc; + int i,j; + int[] rgb_tmp=wk_pickFromRaster_rgb_tmp; + // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) ); + int xdiv = xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X; + int ydiv = ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y; + double xdiv2_reciprocal = 1.0 / xdiv2; + double ydiv2_reciprocal = 1.0 / ydiv2; + + for(j = 0; j < ydiv2; j++ ) { + yw = 102.5 + 5.0 * ((double)j+0.5) * ydiv2_reciprocal; + para21_x_yw=para21*yw+1.0; + para11_x_yw=para11*yw+para12; + para01_x_yw=para01*yw+para02; + ext_pat2_j=ext_pat2[j/ydiv]; + for(i = 0; i < xdiv2; i++ ) { + xw = 102.5 + 5.0 * ((double)i+0.5) * xdiv2_reciprocal; + d = para20*xw + para21_x_yw; + if( d == 0 ){ + throw new NyARException(); + } + xc = (int)((para00*xw + para01_x_yw)/d); + yc = (int)((para10*xw + para11_x_yw)/d); + + + if( xc >= 0 && xc < img_x && yc >= 0 && yc < img_y ) { + image.getPixel(xc, yc, rgb_tmp); + ext_pat2_j_i=ext_pat2_j[i/xdiv]; + + ext_pat2_j_i[0] += rgb_tmp[0];//R + ext_pat2_j_i[1] += rgb_tmp[1];//G + ext_pat2_j_i[2] += rgb_tmp[2];//B + } + } + } + /**/ + int xdiv_x_ydiv=xdiv*ydiv; + for(j =this.height-1; j>=0; j--){ + extpat_j=extpat[j]; + ext_pat2_j=ext_pat2[j]; + for(i = this.width-1; i>=0; i--){ // PRL 2006-06-08. + ext_pat2_j_i=ext_pat2_j[i]; + extpat_j_i=extpat_j[i]; + extpat_j_i[0]=(ext_pat2_j_i[0] / xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_j_i[1]=(ext_pat2_j_i[1] / xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_j_i[2]=(ext_pat2_j_i[2] / xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + } + return true; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O3.java b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O3.java new file mode 100644 index 0000000..19c569f --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARColorPatt_O3.java @@ -0,0 +1,373 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + + + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.NyARRaster; + +/** + * 24ビットカラーのマーカーを保持するために使うクラスです。 + * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 + * 演算順序を含む最適化をしたもの + * + */ +public class NyARColorPatt_O3 implements NyARColorPatt +{ + private static final int AR_PATT_SAMPLE_NUM=64;//#define AR_PATT_SAMPLE_NUM 64 + private int extpat[][][]; + private int width; + private int height; + public NyARColorPatt_O3(int i_width,int i_height) + { + this.width=i_width; + this.height=i_height; + this.extpat=new int[i_height][i_width][3]; + } +// public void setSize(int i_new_width,int i_new_height) +// { +// int array_w=this.extpat[0].length; +// int array_h=this.extpat.length; +// //十分なサイズのバッファがあるか確認 +// if(array_w>=i_new_width && array_h>=i_new_height){ +// //OK 十分だ→サイズ調整のみ +// }else{ +// //足りないよ→取り直し +// this.extpat=new int[i_new_height][i_new_width][3]; +// } +// this.width =i_new_width; +// this.height=i_new_height; +// return; +// } + public int[][][] getPatArray() + { + return extpat; + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + private final NyARMat wk_get_cpara_a=new NyARMat(8,8); + private final NyARMat wk_get_cpara_b=new NyARMat(8,1); + + /** + * @param world + * @param vertex + * @param o_para + * @throws NyARException + */ + private boolean get_cpara(double vertex_0[], double vertex_1[],NyARMat o_para) throws NyARException + { + double world[][]=this.wk_pickFromRaster_world; + NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 ); + double[][] a_array=a.getArray(); + NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 ); + double[][] b_array=b.getArray(); + double[] a_pt0,a_pt1,world_pti; + + for(int i = 0; i < 4; i++ ) { + a_pt0=a_array[i*2]; + a_pt1=a_array[i*2+1]; + world_pti=world[i]; + + a_pt0[0]=world_pti[0];//a->m[i*16+0] = world[i][0]; + a_pt0[1]=world_pti[1];//a->m[i*16+1] = world[i][1]; + a_pt0[2]=1.0;//a->m[i*16+2] = 1.0; + a_pt0[3]=0.0;//a->m[i*16+3] = 0.0; + a_pt0[4]=0.0;//a->m[i*16+4] = 0.0; + a_pt0[5]=0.0;//a->m[i*16+5] = 0.0; + a_pt0[6]=-world_pti[0] * vertex_0[i];//a->m[i*16+6] = -world[i][0] * vertex[i][0]; + a_pt0[7]=-world_pti[1] * vertex_0[i];//a->m[i*16+7] = -world[i][1] * vertex[i][0]; + a_pt1[0]=0.0;//a->m[i*16+8] = 0.0; + a_pt1[1]=0.0;//a->m[i*16+9] = 0.0; + a_pt1[2]=0.0;//a->m[i*16+10] = 0.0; + a_pt1[3]=world_pti[0];//a->m[i*16+11] = world[i][0]; + a_pt1[4]=world_pti[1];//a->m[i*16+12] = world[i][1]; + a_pt1[5]=1.0;//a->m[i*16+13] = 1.0; + a_pt1[6]=-world_pti[0] * vertex_1[i];//a->m[i*16+14] = -world[i][0] * vertex[i][1]; + a_pt1[7]=-world_pti[1] * vertex_1[i];//a->m[i*16+15] = -world[i][1] * vertex[i][1]; + b_array[i*2+0][0]=vertex_0[i];//b->m[i*2+0] = vertex[i][0]; + b_array[i*2+1][0]=vertex_1[i];//b->m[i*2+1] = vertex[i][1]; + } + if(!a.matrixSelfInv()){ + return false; + } + + o_para.matrixMul(a, b); + return true; + } + + // private final double[] wk_pickFromRaster_para=new double[9];//[3][3]; + private final double[][] wk_pickFromRaster_world={//double world[4][2]; + {100.0, 100.0}, + {100.0+10.0,100.0}, + {100.0+10.0,100.0 + 10.0}, + {100.0, 100.0 + 10.0} + }; + /** + * pickFromRaster関数から使う変数です。 + * + */ + private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height) + { + int i,i2; + int[][] pt2; + int[] pt1; + for(i=i_height-1;i>=0;i--){ + pt2=i_ext_pat2[i]; + for(i2=i_width-1;i2>=0;i2--){ + pt1=pt2[i2]; + pt1[0]=0; + pt1[1]=0; + pt1[2]=0; + } + } + } + private final double[][] wk_pickFromRaster_local=new double[2][4]; + private final NyARMat wk_pickFromRaster_cpara=new NyARMat(8,1); + /** + * imageから、i_markerの位置にあるパターンを切り出して、保持します。 + * Optimize:STEP[769->750] + * @param image + * @param i_marker + * @throws Exception + */ + public boolean pickFromRaster(NyARRaster image, NyARMarker i_marker) throws NyARException + { + NyARMat cpara=this.wk_pickFromRaster_cpara; + //localの計算 + int[] x_coord=i_marker.x_coord; + int[] y_coord=i_marker.y_coord; + int[] vertex=i_marker.mkvertex; + double[] local_0=wk_pickFromRaster_local[0];//double local[4][2]; + double[] local_1=wk_pickFromRaster_local[1];//double local[4][2]; + for(int i = 0; i < 4; i++ ) { + local_0[i] = x_coord[vertex[i]]; + local_1[i] = y_coord[vertex[i]]; + } + //xdiv2,ydiv2の計算 + int xdiv2, ydiv2; + int l1,l2; + double w1,w2; + + //x計算 + w1=local_0[0] - local_0[1]; + w2=local_1[0] - local_1[1]; + l1 = (int)(w1*w1+w2*w2); + w1=local_0[2] - local_0[3]; + w2=local_1[2] - local_1[3]; + l2 = (int)(w1*w1+w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + l1=l1/4; + xdiv2 =this.width; + while( xdiv2*xdiv2 < l1 ){ + xdiv2*=2; + } + if( xdiv2 > AR_PATT_SAMPLE_NUM) + { + xdiv2 =AR_PATT_SAMPLE_NUM; + } + + //y計算 + w1=local_0[1] - local_0[2]; + w2=local_1[1] - local_1[2]; + l1 = (int)(w1*w1+ w2*w2); + w1=local_0[3] - local_0[0]; + w2=local_1[3] - local_1[0]; + l2 = (int)(w1*w1+ w2*w2); + if( l2 > l1 ){ + l1 = l2; + } + ydiv2 =this.height; + l1=l1/4; + while( ydiv2*ydiv2 < l1 ){ + ydiv2*=2; + } + if( ydiv2 >AR_PATT_SAMPLE_NUM) + { + ydiv2 = AR_PATT_SAMPLE_NUM; + } + + //cparaの計算 + if(!get_cpara(local_0,local_1,cpara)){ + return false; + } + updateExtpat(image,cpara,xdiv2,ydiv2); + + return true; + } + //かなり大きいワークバッファを取るな…。 + private double[] wk_updateExtpat_para00_xw; + private double[] wk_updateExtpat_para10_xw; + private double[] wk_updateExtpat_para20_xw; + private int[] wk_updateExtpat_rgb_buf; + private int[] wk_updateExtpat_x_rgb_index; + private int[] wk_updateExtpat_y_rgb_index; + private int[] wk_updateExtpat_i_rgb_index; + private int wk_updateExtpat_buffer_size=0; + + /** + * ワークバッファを予約する + * @param i_xdiv2 + */ + private void reservWorkBuffers(int i_xdiv2) + { + if(this.wk_updateExtpat_buffer_sizem[i*3+0]; + para01 = cpara_array[0*3+1][0];//para[i][1] = c->m[i*3+1]; + para02 = cpara_array[0*3+2][0];//para[i][2] = c->m[i*3+2]; + para10 = cpara_array[1*3+0][0];//para[i][0] = c->m[i*3+0]; + para11 = cpara_array[1*3+1][0];//para[i][1] = c->m[i*3+1]; + para12 = cpara_array[1*3+2][0];//para[i][2] = c->m[i*3+2]; + para20 = cpara_array[2*3+0][0];//para[2][0] = c->m[2*3+0]; + para21 = cpara_array[2*3+1][0];//para[2][1] = c->m[2*3+1]; + + + double d,yw; + int xc, yc; + int i,j; + // arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) ); + int xdiv = i_xdiv2/L_WIDTH;//xdiv = xdiv2/Config.AR_PATT_SIZE_X; + int ydiv = i_ydiv2/L_HEIGHT;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y; + + //計算バッファを予約する + this.reservWorkBuffers(i_xdiv2); + double[] para00_xw=this.wk_updateExtpat_para00_xw; + double[] para10_xw=this.wk_updateExtpat_para10_xw; + double[] para20_xw=this.wk_updateExtpat_para20_xw; + int[] x_rgb_index=this.wk_updateExtpat_x_rgb_index; + int[] y_rgb_index=this.wk_updateExtpat_y_rgb_index; + int[] i_rgb_index=this.wk_updateExtpat_i_rgb_index; + int[] rgb_buf=this.wk_updateExtpat_rgb_buf; + double xw; + for(i=0;i=img_x || yc<0 || yc >=img_y){ + continue; + } +// ピクセル値の計算 +// image.getPixel(xc,yc,rgb_buf); +// ext_pat2_j_i=ext_pat2_j[i/xdiv]; +// ext_pat2_j_i[0] += rgb_buf[0];//R +// ext_pat2_j_i[1] += rgb_buf[1];//G +// ext_pat2_j_i[2] += rgb_buf[2];//B + + x_rgb_index[index_num]=xc; + y_rgb_index[index_num]=yc; + i_rgb_index[index_num]=i/xdiv; + index_num++; + } +// //ステップ２．ピクセル配列を取得 + image.getPixelSet(x_rgb_index,y_rgb_index,index_num,rgb_buf); +// //ピクセル値の計算 + for(i=index_num-1;i>=0;i--){ + extpat_j_i=extpat_j[i_rgb_index[i]]; + extpat_j_i[0] += rgb_buf[i*3+0];//R + extpat_j_i[1] += rgb_buf[i*3+1];//G + extpat_j_i[2] += rgb_buf[i*3+2];//B + } + } + /**/ + int xdiv_x_ydiv=xdiv*ydiv; + for(j =L_HEIGHT-1; j>=0; j--){ + extpat_j=L_extpat[j]; + for(i = L_WIDTH-1; i>=0; i--){ // PRL 2006-06-08. + extpat_j_i=extpat_j[i]; + extpat_j_i[0]/=(xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv)); + extpat_j_i[1]/=(xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv)); + extpat_j_i[2]/=(xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv)); + } + } + return; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARDetectMarker.java b/src/jp/nyatla/nyartoolkit/core/NyARDetectMarker.java new file mode 100644 index 0000000..6a00050 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARDetectMarker.java @@ -0,0 +1,302 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + + + + +import jp.nyatla.nyartoolkit.NyARException; + + + + +/** + * イメージからマーカー情報を検出するクラス。 + * このクラスは、arDetectMarker2.cとの置き換えになります。 + * ラベリング済みのラスタデータからマーカー位置を検出して、結果を保持します。 + * + */ +public class NyARDetectMarker { + private static final int AR_AREA_MAX=100000;//#define AR_AREA_MAX 100000 + private static final int AR_AREA_MIN=70;//#define AR_AREA_MIN 70 + +// private final NyARMarker[] marker_holder; //マーカーデータの保持配列 +// private final NyARMarker[] marker_info2_array;//マーカーデータのインデックス配列 +// private int marker_num; + private int width,height; + /** + * 最大i_squre_max個のマーカーを検出するクラスを作成する。 + * @param i_width + * @param i_height + */ + public NyARDetectMarker(int i_width,int i_height) + { + this.width =i_width; + this.height=i_height; + +// this.marker_holder=new NyARMarker[i_squre_max]; +// this.marker_info2_array=new NyARMarker[i_squre_max]; +// //先にマーカーホルダにオブジェクトを作っておく +// for(int i=0;i336] + * @param o_marker + * @param limage + * @param label_ref + * @param label + * @param clip + * @throws NyARException + */ + private final void arGetContour(NyARMarker o_marker,int[][] limage, int[] label_ref,int i_labelnum, NyARLabel i_label) throws NyARException + { + final int[] xcoord=wk_arGetContour_xcoord; + final int[] ycoord=wk_arGetContour_ycoord; + final int[] xdir=wk_arGetContour_xdir; //static int xdir[8] = { 0, 1, 1, 1, 0,-1,-1,-1}; + final int[] ydir=wk_arGetContour_ydir;//static int ydir[8] = {-1,-1, 0, 1, 1, 1, 0,-1}; + //ShortPointer p1;//ARInt16 *p1; + int coord_num; + int sx=0, sy=0, dir; + int dmax, d, v1=0; + int i, j,w; + + int[] limage_j; + j = i_label.clip2; + limage_j=limage[j]; + final int clip1=i_label.clip1; + //p1=ShortPointer.wrap(limage,j*xsize+clip.get());//p1 = &(limage[j*xsize+clip[0]]); + for( i = i_label.clip0; i <= clip1; i++){//for( i = clip[0]; i <= clip[1]; i++, p1++ ) { + w=limage_j[i]; + if(w > 0 && label_ref[w-1] == i_labelnum ) {//if( *p1 > 0 && label_ref[(*p1)-1] == label ) { + sx = i; + sy = j; + break; + } + } + if(i>clip1){//if( i > clip[1] ) { + System.out.println("??? 1");//printf(); + throw new NyARException();//return(-1); + } + +// //マーカーホルダが既に確保済みかを調べる +// if(marker_holder[i_holder_num]==null){ +// //確保していなければ確保 +// marker_holder[i_holder_num]=new NyARMarker(); +// } + + + coord_num=1;//marker_info2->coord_num = 1; + xcoord[0]=sx;//marker_info2->x_coord[0] = sx; + ycoord[0]=sy;//marker_info2->y_coord[0] = sy; + dir = 5; + + int r,c; + c=xcoord[0]; + r=ycoord[0]; + dmax=0; + //本家はdmaxの作成とxcoordの作成を別のループでやってるけど、非効率なので統合 + for(;;){ + //xcoord[1]-xcoord[n]までのデータを作る。 + +// 1個前のxcoordとycoordはループ後半で格納される。 +// c=xcoord[coord_num-1]; +// r=ycoord[coord_num-1]; + //p1 = &(limage[marker_info2->y_coord[marker_info2->coord_num-1] * xsize+ marker_info2->x_coord[marker_info2->coord_num-1]]); + dir = (dir+5)%8; + for(i=0;i<8;i++) { + if(limage[r+ydir[dir]][c+xdir[dir]]>0){//if( p1[ydir[dir]*xsize+xdir[dir]] > 0 ){ + break; + } + dir = (dir+1)%8; + } + if( i == 8 ){ + System.out.println("??? 2");//printf("??? 2\n"); + throw new NyARException();//return(-1); + } +// xcoordとycoordをc,rにも保存 + c= c + xdir[dir];//marker_info2->x_coord[marker_info2->coord_num]= marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir]; + r= r + ydir[dir];//marker_info2->y_coord[marker_info2->coord_num]= marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir]; + xcoord[coord_num]=c;//marker_info2->x_coord[marker_info2->coord_num]= marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir]; + ycoord[coord_num]=r;//marker_info2->y_coord[marker_info2->coord_num]= marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir]; + if(c == sx && r == sy ){ + break; + } + //dmaxの計算 + d=(c-sx)*(c-sx)+(r-sy)*(r-sy); + if( d > dmax ) { + dmax = d; + v1 = coord_num; + } + //終了条件判定 + coord_num++; + if(coord_num == AR_CHAIN_MAX-1){//if( marker_info2.coord_num == Config.AR_CHAIN_MAX-1 ){ + System.out.println("??? 3");//printf("??? 3\n"); + throw new NyARException();//return(-1); + } + } +// +// dmax = 0; +// for(i=1;icoord_num;i++) { +// d = (xcoord[i]-sx)*(xcoord[i]-sx)+ (ycoord[i]-sy)*(ycoord[i]-sy);// d = (marker_info2->x_coord[i]-sx)*(marker_info2->x_coord[i]-sx)+ (marker_info2->y_coord[i]-sy)*(marker_info2->y_coord[i]-sy); +// if( d > dmax ) { +// dmax = d; +// v1 = i; +// } +// } + //NyARMarkerへcoord情報をセット + //coordの並び替えと保存はNyARMarkerへ移動 + o_marker.setCoordXY(v1,coord_num,xcoord,ycoord); + return; + } + + /** + * ARMarkerInfo2 *arDetectMarker2( ARInt16 *limage, int label_num, int *label_ref,int *warea, double *wpos, int *wclip,int area_max, int area_min, double factor, int *marker_num ) + * 関数の代替品 + * ラベリング情報からマーカー一覧を作成してo_marker_listを更新します。 + * 関数はo_marker_listに重なりを除外したマーカーリストを作成します。 + * + * @param i_labeling + * ラベリング済みの情報を持つラベリングオブジェクト + * @param i_factor + * 何かの閾値？ + * @param o_marker_list + * 抽出したマーカーを格納するリスト + * @throws NyARException + */ + public final void detectMarker(NyARLabeling i_labeling,double i_factor,NyARMarkerList o_marker_list) throws NyARException + { + int label_area; + int i; + int xsize, ysize; + NyARLabel[] labels=i_labeling.getLabel(); +// int[] warea =i_labeling.getArea(); + int label_num =i_labeling.getLabelNum(); +// int[][] wclip =i_labeling.getClip(); +// double[] wpos =i_labeling.getPos(); + int[][] limage=i_labeling.getLabelImg(); + int[] label_ref =i_labeling.getLabelRef(); + + //マーカーホルダをリセット + o_marker_list.reset(); +// marker_num=0; + xsize =width; + ysize =height; +// マーカーをmarker_holderに蓄積する。 + NyARMarker current_marker=o_marker_list.getCurrentHolder(); + NyARLabel label_pt; + for(i=0; i AR_AREA_MAX ){ + continue; + } + if( label_pt.clip0 == 1 || label_pt.clip1 == xsize-2 ){//if( wclip[i*4+0] == 1 || wclip[i*4+1] == xsize-2 ){ + continue; + } + if( label_pt.clip2 == 1 || label_pt.clip3 == ysize-2 ){//if( wclip[i*4+2] == 1 || wclip[i*4+3] == ysize-2 ){ + continue; + } + //ret = arGetContour( limage, label_ref, i+1,&(wclip[i*4]), &(marker_info2[marker_num2])); + arGetContour(current_marker,limage, label_ref, i+1,label_pt); + + if(!current_marker.checkSquare(label_area,i_factor,label_pt.pos_x,label_pt.pos_y)){ + //後半で整理するからここはいらない。// marker_holder[marker_num2]=null; + continue; + } +// この3行はcheckSquareの最終段に含める。 +// marker_holder[marker_num2].area = warea[i]; +// marker_holder[marker_num2].pos[0] = wpos[i*2+0]; +// marker_holder[marker_num2].pos[1] = wpos[i*2+1]; + //マーカー検出→次のホルダを取得 + current_marker=o_marker_list.getNextHolder(); + //マーカーリストが上限に達したか確認 + if(current_marker==null){ + break; + } + } + //マーカーリストを整理(重なり処理とかはマーカーリストに責務押し付け) + o_marker_list.updateMarkerArray(); +// 重なり処理かな？ +// double[] pos_j,pos_i; +// for(i=0; i < marker_num2; i++ ){ +// pos_i=marker_holder[i].pos; +// for(j=i+1; j < marker_num2; j++ ) { +// pos_j=marker_holder[j].pos; +// d = (pos_i[0] - pos_j[0])*(pos_i[0] - pos_j[0])+ +// (pos_i[1] - pos_j[1])*(pos_i[1] - pos_j[1]); +// if(marker_holder[i].area >marker_holder[j].area ) { +// if( d + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.*; + + + +public class NyARDetectSquare +{ + private final NyARLabeling labeling; + private final NyARDetectMarker detect; + private NyARParam param; + + /** + * マーカー抽出インスタンスを作ります。 + * @param i_param + */ + public NyARDetectSquare(NyARParam i_param) + { + param=i_param; + //解析オブジェクトを作る + int width=i_param.getX(); + int height=i_param.getY(); + + labeling=new NyARLabeling_O2(width,height); + detect=new NyARDetectMarker(width,height); + } + /** + * ラスタイメージから矩形を検出して、結果o_square_holderへ格納します。 + * @param i_marker + * @param i_number_of_marker + * @param i_square_holder + * @throws NyARException + */ + public void detectSquare(NyARRaster i_image,int i_thresh,NyARSquareList o_square_holder) throws NyARException + { +// number_of_square=0; + + labeling.labeling(i_image, i_thresh); + if(labeling.getLabelNum()<1){ + return; + } + //ここでマーカー配列を作成する。 + detect.detectMarker(labeling,1.0,o_square_holder); + + //マーカー情報をフィルタして、スクエア配列を更新する。 + o_square_holder.updateSquareArray(param); + +// NyARSquare square; +// int j=0; +// for (int i = 0; i =marker_info.length){ +// break; +// } +// } +// number_of_square=j; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARLabeling.java b/src/jp/nyatla/nyartoolkit/core/NyARLabeling.java new file mode 100644 index 0000000..4cd1681 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARLabeling.java @@ -0,0 +1,527 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.raster.*; + + +interface NyARLabeling{ + /** + * 検出したラベルの数を返す + * @return + */ + public int getLabelNum(); + /** + * + * @return + * @throws NyARException + */ + public int[] getLabelRef() throws NyARException; + /** + * 検出したラベル配列 + * @return + * @throws NyARException + */ + public NyARLabel[] getLabel() throws NyARException; + /** + * ラベリング済みイメージを返す + * @return + * @throws NyARException + */ + public int[][] getLabelImg() throws NyARException; + /** + * static ARInt16 *labeling2( ARUint8 *image, int thresh,int *label_num, int **area, double **pos, int **clip,int **label_ref, int LorR ) + * 関数の代替品 + * ラスタimageをラベリングして、結果を保存します。 + * Optimize:STEP[1514->1493] + * @param image + * @param thresh + * @throws NyARException + */ + public void labeling(NyARRaster image,int thresh) throws NyARException; +} + + + + +/** + * NyARLabeling_O2のworkとwork2を可変長にするためのクラス + * + * + */ +class NyARWorkHolder +{ + private final static int ARRAY_APPEND_STEP=256; + public final int[] work; + public final int[][] work2; + private int allocate_size; + /** + * 最大i_holder_size個の動的割り当てバッファを準備する。 + * @param i_holder_size + */ + public NyARWorkHolder(int i_holder_size) + { + //ポインタだけははじめに確保しておく + this.work=new int[i_holder_size]; + this.work2=new int[i_holder_size][]; + this.allocate_size=0; + } + /** + * i_indexで指定した番号までのバッファを準備する。 + * @param i_index + */ + public final void reserv(int i_index) throws NyARException + { + //アロケート済みなら即リターン + if(this.allocate_size>i_index){ + return; + } + //要求されたインデクスは範囲外 + if(i_index>=this.work.length){ + throw new NyARException(); + } + //追加アロケート範囲を計算 + int range=i_index+ARRAY_APPEND_STEP; + if(range>=this.work.length){ + range=this.work.length; + } + //アロケート + for(int i=this.allocate_size;ii_index){ + return; + } + //要求されたインデクスは範囲外 + if(i_index>=this.labels.length){ + throw new NyARException(); + } + //追加アロケート範囲を計算 + int range=i_index+ARRAY_APPEND_STEP; + if(range>=this.labels.length){ + range=this.labels.length; + } + //アロケート + for(int i=this.allocate_size;i + for(int i = 0; i < i_height; i++) { + label_img[i][0]=0; + label_img[i][i_width-1]=0; + } + + + + + } + /** + * 検出したラベルの数を返す + * @return + */ + public int getLabelNum() + { + return label_num; + } + /** + * 検出したエリア配列？ + * @return + * @throws NyARException + */ + public NyARLabel[] getLabel() throws NyARException + { + if(label_num<1){ + throw new NyARException(); + } + return this.label_holder.labels; + } + /** + * + * @return + * @throws NyARException + */ + public int[] getLabelRef() throws NyARException + { + if(label_num<1){ + throw new NyARException(); + } + return work_holder.work; + } + /** + * ラベリング済みイメージを返す + * @return + * @throws NyARException + */ + public int[][] getLabelImg() throws NyARException + { + return glabel_img; + } + //コンストラクタで作ること + private int[] wk_reservLineBuffer_buf=null; + + /** + * static ARInt16 *labeling2( ARUint8 *image, int thresh,int *label_num, int **area, double **pos, int **clip,int **label_ref, int LorR ) + * 関数の代替品 + * ラスタimageをラベリングして、結果を保存します。 + * Optimize:STEP[1514->1493] + * @param image + * @param thresh + * @throws NyARException + */ + public void labeling(NyARRaster image,int thresh) throws NyARException + { + int wk_max; /* work */ + int m,n; /* work */ + int lxsize, lysize; + int thresht3 = thresh * 3; + int i,j,k; + lxsize=image.getWidth();//lxsize = arUtil_c.arImXsize; + lysize=image.getHeight();//lysize = arUtil_c.arImYsize; + //画素数の一致チェック + if(lxsize!=this.width || lysize!=this.height){ + throw new NyARException(); + } + //ラベル数を0に初期化 + this.label_num=0; + + + + int[][] label_img=this.glabel_img; + + + //枠作成はインスタンスを作った直後にやってしまう。 + + int[] work2_pt; + wk_max = 0; + + int label_pixel; + + int[] work=this.work_holder.work; + int[][] work2=this.work_holder.work2; + int[] line_bufferr=this.wk_reservLineBuffer_buf; + + int[] label_img_pt0,label_img_pt1; + for(j = 1; j < lysize - 1; j++) {//for (int j = 1; j < lysize - 1; j++, pnt += poff*2, pnt2 += 2) { + label_img_pt0=label_img[j]; + label_img_pt1=label_img[j-1]; + image.getPixelTotalRowLine(j,line_bufferr); + + for(i = 1; i < lxsize-1; i++) {//for(int i = 1; i < lxsize-1; i++, pnt+=poff, pnt2++) { + //RGBの合計値が閾値より大きいかな？ + if(line_bufferr[i]<=thresht3){ + //pnt1 = ShortPointer.wrap(pnt2, -lxsize);//pnt1 = &(pnt2[-lxsize]); + if(label_img_pt1[i]>0){//if( *pnt1 > 0 ) { + label_pixel=label_img_pt1[i];//*pnt2 = *pnt1; + + + work2_pt=work2[label_pixel-1]; + work2_pt[0]++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1]+=i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2]+=j;//work2[((*pnt2)-1)*7+2] += j; + work2_pt[6]=j;//work2[((*pnt2)-1)*7+6] = j; + }else if(label_img_pt1[i+1]> 0 ) {//}else if( *(pnt1+1) > 0 ) { + if(label_img_pt1[i-1] > 0 ) {//if( *(pnt1-1) > 0 ) { + m = work[label_img_pt1[i+1]-1];//m = work[*(pnt1+1)-1]; + n = work[label_img_pt1[i-1]-1];//n = work[*(pnt1-1)-1]; + if( m > n ){ + label_pixel=n;//*pnt2 = n; + //wk=IntPointer.wrap(work, 0);//wk = &(work[0]); + for(k = 0; k < wk_max; k++) { + if(work[k] == m ){//if( *wk == m ) + work[k]=n;//*wk = n; + } + } + }else if( m < n ) { + label_pixel=m;//*pnt2 = m; + //wk=IntPointer.wrap(work,0);//wk = &(work[0]); + for(k = 0; k < wk_max; k++){ + if(work[k]==n){//if( *wk == n ){ + work[k]=m;//*wk = m; + } + } + }else{ + label_pixel=m;//*pnt2 = m; + } + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++; + work2_pt[1] += i; + work2_pt[2] += j; + work2_pt[6] = j; + }else if( (label_img_pt0[i-1]) > 0 ) {//}else if( *(pnt2-1) > 0 ) { + m = work[(label_img_pt1[i+1])-1];//m = work[*(pnt1+1)-1]; + n = work[label_img_pt0[i-1]-1];//n = work[*(pnt2-1)-1]; + if( m > n ) { + + label_pixel=n;//*pnt2 = n; + for(k = 0; k < wk_max; k++) { + if(work[k]==m){//if( *wk == m ){ + work[k]=n;//*wk = n; + } + } + }else if( m < n ) { + label_pixel=m;//*pnt2 = m; + for(k = 0; k < wk_max; k++) { + if(work[k]==n){//if( *wk == n ){ + work[k]=m;//*wk = m; + } + } + }else{ + label_pixel=m;//*pnt2 = m; + } + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; + }else{ + + label_pixel=label_img_pt1[i+1];//*pnt2 = *(pnt1+1); + + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; + if( work2_pt[3] > i ){//if( work2[((*pnt2)-1)*7+3] > i ){ + work2_pt[3] = i;// work2[((*pnt2)-1)*7+3] = i; + } + work2_pt[6] = j;//work2[((*pnt2)-1)*7+6] = j; + } + }else if( (label_img_pt1[i-1]) > 0 ) {//}else if( *(pnt1-1) > 0 ) { + label_pixel=label_img_pt1[i-1];//*pnt2 = *(pnt1-1); + + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; + if( work2_pt[4] < i ){//if( work2[((*pnt2)-1)*7+4] < i ){ + work2_pt[4] = i;// work2[((*pnt2)-1)*7+4] = i; + } + work2_pt[6] = j;//work2[((*pnt2)-1)*7+6] = j; + }else if(label_img_pt0[i-1] > 0) {//}else if( *(pnt2-1) > 0) { + label_pixel=label_img_pt0[i-1];//*pnt2 = *(pnt2-1); + + work2_pt=work2[label_pixel-1]; + work2_pt[0] ++;//work2[((*pnt2)-1)*7+0] ++; + work2_pt[1] += i;//work2[((*pnt2)-1)*7+1] += i; + work2_pt[2] += j;//work2[((*pnt2)-1)*7+2] += j; + if(work2_pt[4] < i ){//if( work2[((*pnt2)-1)*7+4] < i ){ + work2_pt[4] = i;// work2[((*pnt2)-1)*7+4] = i; + } + }else{ + //現在地までの領域を予約 + this.work_holder.reserv(wk_max); + wk_max++; + work[wk_max-1] = wk_max; + label_pixel=wk_max;//work[wk_max-1] = *pnt2 = wk_max; + work2_pt=work2[wk_max-1]; + work2_pt[0] = 1; + work2_pt[1] = i; + work2_pt[2] = j; + work2_pt[3] = i; + work2_pt[4] = i; + work2_pt[5] = j; + work2_pt[6] = j; + } + label_img_pt0[i]=label_pixel; + }else { + label_img_pt0[i]=0;//*pnt2 = 0; + } + + } + } + j = 1; + for(i = 0; i < wk_max; i++){//for(int i = 1; i <= wk_max; i++, wk++) { + work[i]=(work[i]==i+1)? j++: work[work[i]-1];//*wk = (*wk==i)? j++: work[(*wk)-1]; + } + + int wlabel_num=j - 1;//*label_num = *wlabel_num = j - 1; + + if(wlabel_num==0){//if( *label_num == 0 ) { + //発見数0 + return; + } + + + + //ラベルバッファを予約&初期化 + this.label_holder.init(wlabel_num, lxsize, lysize); +// +// putZero(warea,wlabel_num);//put_zero( (ARUint8 *)warea, *label_num * sizeof(int) ); +// for(i=0;i work2_pt[3] ){ + label_pt.clip0 = work2_pt[3]; + } + if( label_pt.clip1 < work2_pt[4] ){ + label_pt.clip1 = work2_pt[4]; + } + if(label_pt.clip2 > work2_pt[5] ){ + label_pt.clip2 = work2_pt[5]; + } + if( label_pt.clip3 < work2_pt[6] ){ + label_pt.clip3 = work2_pt[6]; + } + } + + for(i = 0; i < wlabel_num; i++ ) {//for(int i = 0; i < *label_num; i++ ) { + label_pt=labels[i]; + label_pt.pos_x /= label_pt.area; + label_pt.pos_y /= label_pt.area; + } + + label_num=wlabel_num; + return; + } +} + diff --git a/src/jp/nyatla/nyartoolkit/core/NyARMarker.java b/src/jp/nyatla/nyartoolkit/core/NyARMarker.java new file mode 100644 index 0000000..182bb5e --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARMarker.java @@ -0,0 +1,278 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + + + + + + + +/** + * typedef struct { + * int area; + * double pos[2]; + * int coord_num; + * int x_coord[AR_CHAIN_MAX]; + * int y_coord[AR_CHAIN_MAX]; + * int vertex[5]; + * } ARMarkerInfo2; + * + */ +class NyARMarker +{ + /** + * メモリブロックのサイズ(32*4=128kb) + */ + private static final int ALLOCATE_PAGE_SIZE=256; + /** + * メモリブロックの初期サイズ + */ + private static final int INITIAL_SIZE=1; + int[] x_coord=new int[INITIAL_SIZE]; + int[] y_coord=new int[INITIAL_SIZE]; + int coord_num; + int area; + final double[] pos=new double[2]; + final int[] mkvertex=new int[5]; + /** + * coordバッファをi_chain_num以上のサイズに再アロケートする。 + * 内容の引継ぎは行われない。 + * @param i_chain_num + */ + private void reallocCoordArray(int i_chain_num) + { + if(x_coord.lengthx_coord[i]; +// wy[i] = marker_ref.y_coord[i];//wy[i] = marker_info2->y_coord[i]; +// } +// for(i=0;icoord_num;i++) { +// marker_ref.x_coord[i] = marker_ref.x_coord[i+v1];//marker_info2->x_coord[i-v1] = marker_info2->x_coord[i]; +// marker_ref.y_coord[i] = marker_ref.y_coord[i+v1];//marker_info2->y_coord[i-v1] = marker_info2->y_coord[i]; +// } +// for(i=0;ix_coord[i-v1+marker_info2->coord_num] = wx[i]; +// marker_ref.y_coord[i-v1+marker_ref.coord_num] = wy[i];//marker_info2->y_coord[i-v1+marker_info2->coord_num] = wy[i]; +// } +// marker_ref.x_coord[marker_ref.coord_num] = marker_ref.x_coord[0];//marker_info2->x_coord[marker_info2->coord_num] = marker_info2->x_coord[0]; +// marker_ref.y_coord[marker_ref.coord_num] = marker_ref.y_coord[0];//marker_info2->y_coord[marker_info2->coord_num] = marker_info2->y_coord[0]; +// marker_ref.coord_num++;//marker_info2->coord_num++; + } + private final NyARVertexCounter wk_checkSquare_wv1=new NyARVertexCounter(); + private final NyARVertexCounter wk_checkSquare_wv2=new NyARVertexCounter(); + /** + * static int arDetectMarker2_check_square( int area, ARMarkerInfo2 *marker_info2, double factor ) + * 関数の代替関数 + * OPTIMIZED STEP [450->415] + * @param i_area + * @param i_factor + * @return + */ + public boolean checkSquare(int i_area,double i_factor,double i_pos_x,double i_pos_y) + { + final int[] l_vertex=mkvertex; + final int[] l_x_coord=x_coord; + final int[] l_y_coord=y_coord; + final NyARVertexCounter wv1=wk_checkSquare_wv1; + final NyARVertexCounter wv2=wk_checkSquare_wv2; + int sx,sy; + int dmax,d,v1; + + int v2;// int wvnum1,wvnum2,v2; + int i; + + final int L_coord_num_m1=this.coord_num-1; + dmax = 0; + v1 = 0; + sx = l_x_coord[0];//sx = marker_info2->x_coord[0]; + sy = l_y_coord[0];//sy = marker_info2->y_coord[0]; + for(i=1;icoord_num-1;i++) { + d = (l_x_coord[i]-sx)*(l_x_coord[i]-sx)+ (l_y_coord[i]-sy)*(l_y_coord[i]-sy); + if( d > dmax ) { + dmax = d; + v1 = i; + } + } + + final double thresh = (i_area/0.75) * 0.01 * i_factor; + + l_vertex[0] = 0; + + if(!wv1.getVertex(l_x_coord,l_y_coord, 0, v1,thresh)){ //if( get_vertex(marker_info2->x_coord, marker_info2->y_coord, 0, v1,thresh, wv1, &wvnum1) < 0 ) { + return false; + } + if(!wv2.getVertex(l_x_coord,l_y_coord,v1,L_coord_num_m1, thresh)) {//if(get_vertex(marker_info2->x_coord, marker_info2->y_coord,v1, marker_info2->coord_num-1, thresh, wv2, &wvnum2) < 0 ) { + return false; + } + + if(wv1.number_of_vertex==1 && wv2.number_of_vertex==1) {//if( wvnum1 == 1 && wvnum2 == 1 ) { + l_vertex[1] = wv1.vertex[0]; + l_vertex[2] = v1; + l_vertex[3] = wv2.vertex[0]; + }else if( wv1.number_of_vertex>1 && wv2.number_of_vertex==0 ) {//}else if( wvnum1 > 1 && wvnum2 == 0 ) { + v2 = v1 / 2; + if(!wv1.getVertex(l_x_coord,l_y_coord,0, v2, thresh)) { + return false; + } + if(!wv2.getVertex(l_x_coord,l_y_coord,v2, v1, thresh)) { + return false; + } + if(wv1.number_of_vertex==1 && wv2.number_of_vertex==1 ) { + l_vertex[1] = wv1.vertex[0]; + l_vertex[2] = wv2.vertex[0]; + l_vertex[3] = v1; + }else{ + return false; + } + }else if(wv1.number_of_vertex==0 && wv2.number_of_vertex> 1 ) { + v2 = (v1 + this.coord_num-1) / 2; + + if(!wv1.getVertex(l_x_coord,l_y_coord,v1, v2, thresh)) { + return false; + } + if(!wv2.getVertex(l_x_coord,l_y_coord,v2,L_coord_num_m1, thresh)) { + return false; + } + if( wv1.number_of_vertex==1 && wv2.number_of_vertex==1 ) { + l_vertex[1] = v1; + l_vertex[2] = wv1.vertex[0]; + l_vertex[3] = wv2.vertex[0]; + } + else { + return false; + } + } + else { + return false; + } + l_vertex[4] =L_coord_num_m1;//この値使ってるの？ + // + area = i_area; + pos[0] = i_pos_x; + pos[1] = i_pos_y; +// marker_holder[marker_num2].pos[1] = wpos[i*2+1]; + return true; + } +} + + + +/** + * get_vertex関数を切り離すためのクラス + * + */ +final class NyARVertexCounter +{ + public final int[] vertex=new int[10];//5まで削れる + public int number_of_vertex; + private double thresh; + private int[] x_coord; + private int[] y_coord; + + public boolean getVertex(int[] i_x_coord, int[] i_y_coord, int st, int ed,double i_thresh) + { + this.number_of_vertex=0; + this.thresh=i_thresh; + this.x_coord=i_x_coord; + this.y_coord=i_y_coord; + return get_vertex(st,ed); + } + /** + * static int get_vertex( int x_coord[], int y_coord[], int st, int ed,double thresh, int vertex[], int *vnum) + * 関数の代替関数 + * @param x_coord + * @param y_coord + * @param st + * @param ed + * @param thresh + * @return + */ + private boolean get_vertex(int st, int ed) + { + double d, dmax; + double a, b, c; + int i, v1=0; + final int[] lx_coord=this.x_coord; + final int[] ly_coord=this.y_coord; + a = ly_coord[ed] - ly_coord[st]; + b = lx_coord[st] - lx_coord[ed]; + c = lx_coord[ed]*ly_coord[st] - ly_coord[ed]*lx_coord[st]; + dmax = 0; + for(i=st+1;i dmax ) { + dmax = d*d; + v1 = i; + } + } + if( dmax/(a*a+b*b) > thresh ) { + if(!get_vertex(st, v1)){ + return false; + } + if(number_of_vertex > 5 ){ + return false; + } + vertex[number_of_vertex] = v1;//vertex[(*vnum)] = v1; + number_of_vertex++;//(*vnum)++; + + if(!get_vertex(v1, ed)){ + return false; + } + } + return true; + } +} + diff --git a/src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java b/src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java new file mode 100644 index 0000000..6e03c2f --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARMarkerList.java @@ -0,0 +1,148 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +/** + * このクラスは、detectMarkerがマーカーオブジェクトの蓄積に使うクラスです。 + * 実体を伴うマーカーホルダと、これを参照するマーカーアレイを持ちます。 + * + * マーカーアレイはマーカーホルダに存在するマーカーリストを特定の条件でフィルタした + * 結果を格納します。 + * + * 一度作られたマーカーホルダは繰り返し使用されます。 + * + * + */ +public class NyARMarkerList +{ + private int marker_holder_num;//marker_holderの使用中の数 + protected int marker_array_num;//marker_arrayの有効な数 + protected final NyARMarker[] marker_holder;//マーカーデータの保持配列 + protected final NyARMarker[] marker_array; //マーカーデータのインデックス配列 + /** + * 派生データ型をラップするときに使う + * @param i_holder + * 値の保持配列。全要素に実体を割り当てる必要がある。 + */ + protected NyARMarkerList(NyARMarker[] i_holder) + { + this.marker_holder=i_holder; + this.marker_array =new NyARMarker[i_holder.length]; + this.marker_array_num =0; + this.marker_holder_num =0; + } + public NyARMarkerList(int i_number_of_holder) + { + this.marker_holder=new NyARMarker[i_number_of_holder]; + //先にマーカーホルダにオブジェクトを作っておく + for(int i=0;i=this.marker_holder.length){ + throw new NyARException(); + } + return this.marker_holder[this.marker_holder_num]; + } + /** + * マーカーホルダの現在位置を1つ進めて、そのホルダを返す。 + * この関数を実行すると、使用中のマーカーホルダの数が1個増える。 + * @return + * 空いているマーカーホルダが無ければnullを返します。 + * + */ + public final NyARMarker getNextHolder() + { + //現在位置が終端位置ならnullを返す。 + if(this.marker_holder_num+1>=this.marker_holder.length){ + this.marker_holder_num=this.marker_holder.length; + return null; + } + this.marker_holder_num++; + return this.marker_holder[this.marker_holder_num]; + } + /** + * マーカーアレイのi_indexの要素を返す。 + * @param i_index + * @return + * @throws NyARException + */ + public final NyARMarker getMarker(int i_index) throws NyARException + { + if(i_index>=marker_array_num){ + throw new NyARException(); + } + return this.marker_array[i_index]; + } + /** + * マーカーアレイの要素数を返す。 + * @return + */ + public final int getMarkerNum() + { + return marker_array_num; + } + /** + * マーカーアレイの要素数と、マーカーホルダの現在位置をリセットする。 + * @return + */ + public final void reset() + { + this.marker_array_num=0; + this.marker_holder_num=0; + } + /** + * マーカーホルダに格納済みのマーカーから重なっているのものを除外して、 + * マーカーアレイにフィルタ結果を格納します。 + * [[この関数はマーカー検出処理と密接に関係する関数です。 + * NyARDetectMarkerクラス以外から呼び出さないで下さい。]] + * メモ:この関数はmarker_holderの内容を変化させまするので注意。 + */ + public final void updateMarkerArray() + { + //重なり処理かな？ + int i; + double d; + double[] pos_j,pos_i; +// NyARMarker[] marker_holder; + for(i=0; i < this.marker_holder_num; i++ ){ + pos_i=marker_holder[i].pos; + for(int j=i+1; j < this.marker_holder_num; j++ ) { + pos_j=marker_holder[j].pos; + d = (pos_i[0] - pos_j[0])*(pos_i[0] - pos_j[0])+ + (pos_i[1] - pos_j[1])*(pos_i[1] - pos_j[1]); + if(marker_holder[i].area >marker_holder[j].area ) { + if( d + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + + + +/** + * ARMat構造体に対応するクラス + * typedef struct { + * double *m; + * int row; + * int clm; + * }ARMat; + * + */ +public class NyARMat{ + /** + * 配列サイズと行列サイズは必ずしも一致しないことに注意 + * 返された配列のサイズを行列の大きさとして使わないこと！ + * + */ + protected double[][] m; + private int clm,row; + /** + * デフォルトコンストラクタは機能しません。 + * @throws NyARException + */ + protected NyARMat() throws NyARException + { + throw new NyARException(); + } + public NyARMat(int i_row,int i_clm) + { + m=new double[i_row][i_clm]; + clm=i_clm; + row=i_row; + } + /** + * i_row x i_clmサイズの行列を格納できるように行列サイズを変更します。 + * 実行後、行列の各値は不定になります。 + * @param i_row + * @param i_clm + */ + public void realloc(int i_row,int i_clm) + { + if(i_row<=this.m.length && i_clm<=this.m[0].length) + { + //十分な配列があれば何もしない。 + }else{ + //不十分なら取り直す。 + m=new double[i_row][i_clm]; + } + this.clm=i_clm; + this.row=i_row; + } + public int getClm() + { + return clm; + } + public int getRow() + { + return row; + } + /** + * 行列をゼロクリアする。 + */ + public void zeroClear() + { + int i,i2; + //For順変更OK + for(i=row-1;i>=0;i--){ + for(i2=clm-1;i2>=0;i2--){ + m[i][i2]=0.0; + } + } + } + /** + * i_copy_fromの内容を自分自身にコピーします。 + * 高さ・幅は同一で無いと失敗します。 + * @param i_copy_from + */ + public void copyFrom(NyARMat i_copy_from)throws NyARException + { + //サイズ確認 + if(this.row!=i_copy_from.row ||this.clm!=i_copy_from.clm) + { + throw new NyARException(); + } + //値コピー + for(int r=this.row-1;r>=0;r--){ + for(int c=this.clm-1;c>=0;c--){ + this.m[r][c]=i_copy_from.m[r][c]; + } + } + } + + public double[][] getArray() + { + return m; + } +// public void getRowVec(int i_row,NyARVec o_vec) +// { +// o_vec.set(this.m[i_row],this.clm); +// } + /** + * aとbの積を自分自身に格納する。arMatrixMul()の代替品 + * @param a + * @param b + * @throws NyARException + */ + public void matrixMul(NyARMat a, NyARMat b) throws NyARException + { + if(a.clm != b.row || this.row != a.row || this.clm != b.clm){ + throw new NyARException(); + } + double w; + int r,c,i; + double[][] am=a.m,bm=b.m,dm=this.m; + //For順変更禁止 + for(r = 0; r < this.row; r++){ + for(c = 0; c < this.clm; c++){ + w=0.0;//dest.setARELEM0(r, c,0.0); + for(i = 0; i < a.clm; i++){ + w+=am[r][i]*bm[i][c];//ARELEM0(dest, r, c) += ARELEM0(a, r, i) * ARELEM0(b, i, c); + } + dm[r][c]=w; + } + } + } + private int[] wk_nos_matrixSelfInv=new int[50]; +// private final static double matrixSelfInv_epsl=1.0e-10; + /** + * i_targetを逆行列に変換する。arMatrixSelfInv()と、arMatrixSelfInv_minv()関数を合成してあります。 + * OPTIMIZE STEP[485->422] + * @param i_target + * 逆行列にする行列 + * @return + * 逆行列があればTRUE/無ければFALSE + * + * @throws NyARException + */ + public boolean matrixSelfInv() throws NyARException + { + double[][] ap=this.m; + int dimen=this.row; + int dimen_1=dimen-1; + double[] ap_n,ap_ip,ap_i;//wap; + int j,ip,nwork; + int[] nos=wk_nos_matrixSelfInv;//この関数で初期化される。 + //double epsl; + double p,pbuf,work; + + /* check size */ + switch(dimen){ + case 0: + throw new NyARException(); + case 1: + ap[0][0]=1.0/ap[0][0];//*ap = 1.0 / (*ap); + return true;/* 1 dimension */ + } + + for(int n = 0; n < dimen ; n++){ + nos[n] = n; + } + + /* nyatla memo + * ipが定まらないで計算が行われる場合があるので挿入。 + * ループ内で0初期化していいかが判らない。 + */ + ip=0; + //For順変更禁止 + for(int n=0; n=0; r--){ + for(c =this.clm-1;c>=0; c--) + { + dest_m[r][c]=src_m[r][c]; + } + } + } + public NyARMat matrixAllocDup() throws NyARException + { + NyARMat result=new NyARMat(this.row,this.clm); + //コピー + int r,c; + double[][] dest_m,src_m; + dest_m=result.m; + src_m =this.m; + //コピーはFor順を変えてもOK + for(r = this.row-1; r>=0; r--){ + for(c =this.clm-1;c>=0; c--) + { + dest_m[r][c]=src_m[r][c]; + } + } + return result; + } + /** + * arMatrixInv関数の代替品です。 + * destにsourceの逆行列を返します。 + * @param dest + * @param source + * @throws NyARException + */ + public static void matrixInv(NyARMat dest,NyARMat source) throws NyARException + { + NyARException.trap("未チェックのパス"); + dest.matrixDup(source); + + NyARException.trap("未チェックのパス"); + dest.matrixSelfInv(); + } + public NyARMat matrixAllocInv() throws NyARException + { + NyARException.trap("未チェックのパス"); + NyARMat result=matrixAllocDup(); + + NyARException.trap("未チェックのパス"); + result.matrixSelfInv(); + return result; + } + /** + * dim x dim の単位行列を作る。 + * @param dim + * @return + * @throws NyARException + */ + public static NyARMat matrixAllocUnit(int dim) throws NyARException + { + NyARException.trap("未チェックのパス"); + NyARMat result = new NyARMat(dim, dim); + NyARException.trap("未チェックのパス"); + NyARMat.matrixUnit(result); + return result; + } + /** + * arMatrixDispの代替品 + * @param m + * @return + */ + public int matrixDisp() throws NyARException + { + NyARException.trap("未チェックのパス"); + System.out.println(" === matrix ("+row+","+clm+") ===");//printf(" === matrix (%d,%d) ===\n", m->row, m->clm); + for(int r = 0; r < row; r++){//for(int r = 0; r < m->row; r++) { + System.out.print(" |");//printf(" |"); + for(int c = 0; c < clm; c++) {//for(int c = 0; c < m->clm; c++) { + System.out.print(" "+m[r][c]);//printf(" %10g", ARELEM0(m, r, c)); + } + System.out.println(" |");//printf(" |\n"); + } + System.out.println(" ======================");//printf(" ======================\n"); + return 0; + } + private final static double PCA_EPS=1e-6; //#define EPS 1e-6 + private final static int PCA_MAX_ITER=100; //#define MAX_ITER 100 + private final static double PCA_VZERO=1e-16; //#define VZERO 1e-16 + /** + * static int EX( ARMat *input, ARVec *mean )の代替関数 + * Optimize:STEP:[144->110] + * @param input + * @param mean + * @return + * @throws NyARException + */ + private void PCA_EX(NyARVec mean) throws NyARException + { + int lrow,lclm; + int i,i2; + lrow = this.row; + lclm = this.clm; + double lm[][]=this.m; + + if(lrow <= 0 || lclm <= 0){ + throw new NyARException(); + } + if( mean.getClm() != lclm ){ + throw new NyARException(); + } +// double[] mean_array=mean.getArray(); +// mean.zeroClear(); + final double[] mean_array=mean.getArray(); + double w; + //For順変更禁止 + for(i2=0;i2v[i] /= row; + } + } + /** + * static int CENTER( ARMat *inout, ARVec *mean )の代替関数 + * @param inout + * @param mean + * @return + */ + private static void PCA_CENTER(NyARMat inout, NyARVec mean) throws NyARException + { + double[] v; + int row, clm; + + row = inout.getRow(); + clm = inout.getClm(); + if(mean.getClm()!= clm){ + throw new NyARException(); + } + double[][] im=inout.m; + double[] im_i; + double w0,w1; + v = mean.getArray(); + //特にパフォーマンスが劣化するclm=1と2ときだけ、別パスで処理します。 + switch(clm){ + case 1: + w0=v[0]; + for(int i = 0; i < row; i++ ){ + im[i][0]-=w0; + } + break; + case 2: + w0=v[0]; + w1=v[1]; + for(int i = 0; i < row; i++ ){ + im_i=im[i]; + im_i[0]-=w0; + im_i[1]-=w1; + } + break; + default: + for(int i = 0; i < row; i++ ){ + im_i=im[i]; + for(int j = 0; j < clm; j++ ){ + //*(m++) -= *(v++); + im_i[j]-=v[j]; + } + } + } + return; + } + /** + * int x_by_xt( ARMat *input, ARMat *output )の代替関数 + * @param input + * @param output + * @throws NyARException + */ + private static void PCA_x_by_xt( NyARMat input, NyARMat output) throws NyARException + { + NyARException.trap("動作未チェック/配列化未チェック"); + int row, clm; +// double[][] out; + double[] in1,in2; + + NyARException.trap("未チェックのパス"); + row = input.row; + clm = input.clm; + NyARException.trap("未チェックのパス"); + if( output.row != row || output.clm != row ){ + throw new NyARException(); + } + +// out = output.getArray(); + for(int i = 0; i < row; i++ ) { + for(int j = 0; j < row; j++ ) { + if( j < i ) { + NyARException.trap("未チェックのパス"); + output.m[i][j]=output.m[j][i];//*out = output->m[j*row+i]; + }else{ + NyARException.trap("未チェックのパス"); + in1=input.m[i];//input.getRowArray(i);//in1 = &(input->m[clm*i]); + in2=input.m[j];//input.getRowArray(j);//in2 = &(input->m[clm*j]); + output.m[i][j]=0;//*out = 0.0; + for(int k = 0; k < clm; k++ ){ + output.m[i][j]+=(in1[k]*in2[k]);//*out += *(in1++) * *(in2++); + } + } + // out.incPtr(); + } + } + } + /** + * static int xt_by_x( ARMat *input, ARMat *output )の代替関数 + * Optimize:2008.04.19 + * @param input + * @param i_output + * @throws NyARException + */ + private static void PCA_xt_by_x(NyARMat input, NyARMat i_output) throws NyARException + { + double[] in; + int row, clm; + + row = input.row; + clm = input.clm; + if(i_output.row!= clm || i_output.clm != clm ){ + throw new NyARException(); + } + + int k,j; + double[][] out_m=i_output.m; + double w; + for(int i = 0; i < clm; i++ ) { + for(j = 0; j < clm; j++ ) { + if( j < i ) { + out_m[i][j]=out_m[j][i];//*out = output->m[j*clm+i]; + }else{ + w=0.0;//*out = 0.0; + for(k = 0; k < row; k++ ){ + in=input.m[k];//in=input.getRowArray(k); + w+=(in[i]*in[j]);//*out += *in1 * *in2; + } + out_m[i][j]=w; + } + } + } + } + private final NyARVec wk_PCA_QRM_ev=new NyARVec(1); + /** + * static int QRM( ARMat *a, ARVec *dv )の代替関数 + * @param a + * @param dv + * @throws NyARException + */ + private void PCA_QRM(NyARVec dv) throws NyARException + { + double w, t, s, x, y, c; + int dim, iter; + double[] dv_array=dv.getArray(); + + dim = this.row; + if( dim != this.clm || dim < 2 ){ + throw new NyARException(); + } + if( dv.getClm() != dim ){ + throw new NyARException(); + } + + NyARVec ev = this.wk_PCA_QRM_ev; + ev.realloc(dim); + double[] ev_array=ev.getArray(); + if( ev == null ){ + throw new NyARException(); + } + final double[][] L_m=this.m; + this.vecTridiagonalize(dv,ev,1); + + ev_array[0]=0.0;//ev->v[0] = 0.0; + for(int h = dim-1; h > 0; h-- ) { + int j = h; + while(j>0 && Math.abs(ev_array[j]) > PCA_EPS*(Math.abs(dv_array[j-1])+Math.abs(dv_array[j]))){// while(j>0 && fabs(ev->v[j]) > EPS*(fabs(dv->v[j-1])+fabs(dv->v[j]))) j--; + j--; + } + if( j == h ){ + continue; + } + iter = 0; + do{ + iter++; + if( iter > PCA_MAX_ITER ){ + break; + } + w = (dv_array[h-1] - dv_array[h]) / 2;//w = (dv->v[h-1] - dv->v[h]) / 2;//ここ？ + t = ev_array[h] * ev_array[h];//t = ev->v[h] * ev->v[h]; + s = Math.sqrt(w*w+t); + if( w < 0 ){ + s = -s; + } + x=dv_array[j] - dv_array[h] + t/(w+s);//x = dv->v[j] - dv->v[h] + t/(w+s); + y=ev_array[j+1];//y = ev->v[j+1]; + for(int k = j; k < h; k++ ){ + if( Math.abs(x) >= Math.abs(y)){ + if( Math.abs(x) > PCA_VZERO ) { + t = -y / x; + c = 1 / Math.sqrt(t*t+1); + s = t * c; + }else{ + c = 1.0; + s = 0.0; + } + }else{ + t = -x / y; + s = 1.0 / Math.sqrt(t*t+1); + c = t * s; + } + w = dv_array[k] - dv_array[k+1];//w = dv->v[k] - dv->v[k+1]; + t = (w * s + 2 * c * ev_array[k+1]) * s;//t = (w * s + 2 * c * ev->v[k+1]) * s; + dv_array[k]-=t;//dv->v[k] -= t; + dv_array[k+1]+=t;//dv->v[k+1] += t; + if( k > j){ + NyARException.trap("未チェックパス");{ + ev_array[k]=c * ev_array[k] - s * y;//ev->v[k] = c * ev->v[k] - s * y; + } + } + ev_array[k+1]+=s * (c * w - 2 * s * ev_array[k+1]);//ev->v[k+1] += s * (c * w - 2 * s * ev->v[k+1]); + + for(int i = 0; i < dim; i++ ){ + x = L_m[k][i];//x = a->m[k*dim+i]; + y = L_m[k+1][i];//y = a->m[(k+1)*dim+i]; + L_m[k][i]=c * x - s * y;//a->m[k*dim+i] = c * x - s * y; + L_m[k+1][i]=s * x + c * y;//a->m[(k+1)*dim+i] = s * x + c * y; + } + if( k < h-1 ) { + NyARException.trap("未チェックパス");{ + x = ev_array[k+1];//x = ev->v[k+1]; + y =-s*ev_array[k+2];//y = -s * ev->v[k+2]; + ev_array[k+2]*=c;//ev->v[k+2] *= c; + } + } + } + }while(Math.abs(ev_array[h]) > PCA_EPS*(Math.abs(dv_array[h-1])+Math.abs(dv_array[h]))); + } + for(int k = 0; k < dim-1; k++ ) { + int h = k; + t=dv_array[h];//t = dv->v[h]; + for(int i = k+1; i < dim; i++ ){ + if(dv_array[i] > t ){//if( dv->v[i] > t ) { + h = i; + t=dv_array[h];//t = dv->v[h]; + } + } + dv_array[h]=dv_array[k];//dv->v[h] = dv->v[k]; + dv_array[k]=t;//dv->v[k] = t; + this.flipRow(h,k); + } + } + /** + * i_row_1番目の行と、i_row_2番目の行を入れ替える。 + * @param i_row_1 + * @param i_row_2 + */ + private void flipRow(int i_row_1,int i_row_2) + { + int i; + double w; + double[] r1=this.m[i_row_1],r2=this.m[i_row_2]; + //For順変更OK + for(i=clm-1;i>=0;i--){ + w=r1[i]; + r1[i]=r2[i]; + r2[i]=w; + } + } + /** + * static int EV_create( ARMat *input, ARMat *u, ARMat *output, ARVec *ev )の代替関数 + * @param input + * @param u + * @param output + * @param ev + * @throws NyARException + */ + private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output, NyARVec ev) throws NyARException + { + NyARException.trap("未チェックのパス"); + int row, clm; + row = input.row;//row = input->row; + clm = input.clm;//clm = input->clm; + if( row <= 0 || clm <= 0 ){ + throw new NyARException(); + } + if( u.row != row || u.clm != row ){//if( u->row != row || u->clm != row ){ + throw new NyARException(); + } + if( output.row != row || output.clm != clm ){//if( output->row != row || output->clm != clm ){ + throw new NyARException(); + } + if( ev.getClm()!= row ){//if( ev->clm != row ){ + throw new NyARException(); + } + double[][] m,in; + double[] m1,ev_array; + double sum, work; + + NyARException.trap("未チェックのパス"); + m =output.m;//m = output->m; + in=input.m; + int i; + ev_array=ev.getArray(); + for(i = 0; i < row; i++ ) { + NyARException.trap("未チェックのパス"); + if( ev_array[i]v[i] < VZERO ){ + break; + } + NyARException.trap("未チェックのパス"); + work = 1 / Math.sqrt(Math.abs(ev_array[i]));//work = 1 / sqrt(fabs(ev->v[i])); + for(int j = 0; j < clm; j++ ) { + sum = 0.0; + m1=u.m[i];//m1 = &(u->m[i*row]); + // m2=input.getPointer(j);//m2 = &(input->m[j]); + for(int k = 0; k < row; k++ ) { + sum+=m1[k]+in[k][j];//sum += *m1 * *m2; + // m1.incPtr(); //m1++; + // m2.addPtr(clm);//m2 += clm; + } + m1[j]=sum * work;//*(m++) = sum * work; + // {//*(m++) = sum * work; + // m.set(sum * work); + // m.incPtr();} + } + } + for( ; i < row; i++ ) { + NyARException.trap("未チェックのパス"); + ev_array[i]=0.0;//ev->v[i] = 0.0; + for(int j = 0; j < clm; j++ ){ + m[i][j]=0.0; + // m.set(0.0);//*(m++) = 0.0; + // m.incPtr(); + } + } + } + private NyARMat wk_PCA_PCA_u=null; + /** + * static int PCA( ARMat *input, ARMat *output, ARVec *ev ) + * + * @param output + * @param o_ev + * @throws NyARException + */ + private void PCA_PCA(NyARMat o_output, NyARVec o_ev) throws NyARException + { + + int l_row, l_clm, min; + double[] ev_array=o_ev.getArray(); + + l_row =this.row;//row = input->row; + l_clm =this.clm;//clm = input->clm; + min =(l_clm < l_row)? l_clm: l_row; + if( l_row < 2 || l_clm < 2 ){ + throw new NyARException(); + } + if( o_output.clm != this.clm){//if( output->clm != input->clm ){ + throw new NyARException(); + } + if( o_output.row!= min ){//if( output->row != min ){ + throw new NyARException(); + } + if( o_ev.getClm() != min ){//if( ev->clm != min ){ + throw new NyARException(); + } + + NyARMat u;// u =new NyARMat( min, min ); + if(this.wk_PCA_PCA_u==null){ + u=new NyARMat( min, min ); + this.wk_PCA_PCA_u=u; + }else{ + u=this.wk_PCA_PCA_u; + u.realloc(min,min); + } + + + if( l_row < l_clm ){ + NyARException.trap("未チェックのパス"); + PCA_x_by_xt( this, u );//if(x_by_xt( input, u ) < 0 ) { + }else{ + PCA_xt_by_x( this, u );//if(xt_by_x( input, u ) < 0 ) { + } + u.PCA_QRM(o_ev); + + double[][] m1,m2; + if( l_row < l_clm ) { + NyARException.trap("未チェックのパス"); + PCA_EV_create( this, u, o_output, o_ev ); + }else{ + m1=u.m;//m1 = u->m; + m2=o_output.m;//m2 = output->m; + int i; + for(i = 0; i < min; i++){ + if( ev_array[i] < PCA_VZERO){//if( ev->v[i] < VZERO ){ + break; + } + for(int j = 0; j < min; j++ ){ + m2[i][j]=m1[i][j];//*(m2++) = *(m1++); + } + } + for( ; i < min; i++){//for( ; i < min; i++){ + //コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス"); + ev_array[i]=0.0;//ev->v[i] = 0.0; + for(int j = 0; j < min; j++ ){ + m2[i][j]=0.0;//*(m2++) = 0.0; + } + } + } + } + private NyARMat wk_work_matrixPCA=null; + /** + * int arMatrixPCA( ARMat *input, ARMat *evec, ARVec *ev, ARVec *mean ); + * 関数の置き換え。input引数がthisになる。 + * Optimize:2008.04.19 + * @param o_evec + * @param o_ev + * + * @param mean + * @throws NyARException + */ + public void matrixPCA(NyARMat o_evec, NyARVec o_ev, NyARVec mean) throws NyARException + { + double srow, sum; + int l_row, l_clm; + int check; + + + l_row=this.row;//row = input->row; + l_clm=this.clm;//clm = input->clm; + check = (l_row < l_clm)? l_row: l_clm; + if( l_row < 2 || l_clm < 2 ){ + throw new NyARException(); + } + if( o_evec.clm != l_clm || o_evec.row != check ){//if( evec->clm != input->clm || evec->row != check ){ + throw new NyARException(); + } + if( o_ev.getClm() != check ){//if( ev->clm != check ){ + throw new NyARException(); + } + if( mean.getClm() != l_clm){//if( mean->clm != input->clm ){ + throw new NyARException(); + } + + //自分の内容をワークにコピー(高速化の為に、1度作ったインスタンスは使いまわす) + NyARMat work; + if(this.wk_work_matrixPCA==null){ + work=this.matrixAllocDup(); + this.wk_work_matrixPCA=work; + }else{ + work=this.wk_work_matrixPCA; + work.matrixDup(this);//arMatrixAllocDup( input );work = arMatrixAllocDup( input ); + } + + + srow = Math.sqrt((double)l_row); + work.PCA_EX(mean ); + + PCA_CENTER(work,mean); + + + int i,j; + //For順変更OK + for(i=0; im[i] /= srow; + } + } + + work.PCA_PCA(o_evec, o_ev); + + sum = 0.0; + double[] ev_array=o_ev.getArray(); + int ev_clm=o_ev.getClm(); + //For順変更禁止 + for(i=0;iclm; i++ ){ + sum+=ev_array[i];//sum += ev->v[i]; + } + //For順変更禁止 + for(i=0;iclm; i++ ){ + ev_array[i]/=sum;//ev->v[i] /= sum; + } + } + + /*int arMatrixPCA2( ARMat *input, ARMat *evec, ARVec *ev );*/ + public static void arMatrixPCA2( NyARMat input, NyARMat evec, NyARVec ev) throws NyARException + { + NyARException.trap("未チェックのパス"); + NyARMat work; + // double srow; // unreferenced + double sum; + int row, clm; + int check; + + row=input.row;//row = input->row; + clm=input.clm;//clm = input->clm; + check = (row < clm)? row: clm; + if( row < 2 || clm < 2 ){ + throw new NyARException(); + } + if( evec.getClm()!= input.clm|| evec.row!=check){//if( evec->clm != input->clm || evec->row != check ){ + throw new NyARException(); + } + if( ev.getClm() != check ){//if( ev->clm != check ){ + throw new NyARException(); + } + + NyARException.trap("未チェックのパス"); + work =input.matrixAllocDup(); + + NyARException.trap("未チェックパス"); + work.PCA_PCA(evec, ev );//rval = PCA( work, evec, ev ); + sum = 0.0; + double[] ev_array=ev.getArray(); + for(int i = 0; i < ev.getClm(); i++ ){//for( i = 0; i < ev->clm; i++ ){ + NyARException.trap("未チェックパス"); + sum+=ev_array[i];//sum += ev->v[i]; + } + for(int i = 0; i < ev.getClm(); i++ ){//for(int i = 0; i < ev->clm; i++ ){ + NyARException.trap("未チェックパス"); + ev_array[i]/=sum;//ev->v[i] /= sum; + } + return; + } + public static NyARMat matrixAllocMul(NyARMat a, NyARMat b) throws NyARException + { + NyARException.trap("未チェックのパス"); + NyARMat dest=new NyARMat(a.row, b.clm); + NyARException.trap("未チェックのパス"); + dest.matrixMul(a, b); + return dest; + } + /*static double mdet(double *ap, int dimen, int rowa)*/ + private static double Det_mdet(double[][] ap, int dimen, int rowa) throws NyARException + { + NyARException.trap("動作未チェック/配列化未チェック"); + double det = 1.0; + double work; + int is = 0; + int mmax; + + for(int k = 0; k < dimen - 1; k++) { + mmax = k; + for(int i = k + 1; i < dimen; i++){ +// if (Math.abs(arMatrixDet_MATRIX_get(ap, i, k, rowa)) > Math.abs(arMatrixDet_MATRIX_get(ap, mmax, k, rowa))){ + if (Math.abs(ap[i][k]) > Math.abs(ap[mmax][k])){ + mmax = i; + } + } + if(mmax != k) { + for (int j = k; j < dimen; j++) { + work = ap[k][j];//work = MATRIX(ap, k, j, rowa); + ap[k][j]=ap[mmax][j];//MATRIX(ap, k, j, rowa) = MATRIX(ap, mmax, j, rowa); + ap[mmax][j]=work;//MATRIX(ap, mmax, j, rowa) = work; + } + is++; + } + for(int i = k + 1; i < dimen; i++) { + work = ap[i][k]/ ap[k][k];//work = arMatrixDet_MATRIX_get(ap, i, k, rowa) / arMatrixDet_MATRIX_get(ap, k, k, rowa); + for (int j = k + 1; j < dimen; j++){ + //MATRIX(ap, i, j, rowa) -= work * MATRIX(ap, k, j, rowa); + ap[i][j]-=work * ap[k][j]; + } + } + } + for(int i = 0; i < dimen; i++){ + det=ap[i][i];//det *= MATRIX(ap, i, i, rowa); + } + for(int i = 0; i < is; i++){ + det *= -1.0; + } + return det; + } + /*double arMatrixDet(ARMat *m);*/ + public static double arMatrixDet(NyARMat m) throws NyARException + { + NyARException.trap("動作未チェック/配列化未チェック"); + if(m.row != m.clm){ + return 0.0; + } + return Det_mdet(m.getArray(), m.row, m.clm);//return mdet(m->m, m->row, m->row); + } + private final NyARVec wk_vecTridiagonalize_vec=new NyARVec(0); + private final NyARVec wk_vecTridiagonalize_vec2=new NyARVec(0); + /** + * arVecTridiagonalize関数の代替品 + * a,d,e間で演算をしてる。何をどうしているかはさっぱりさっぱり + * @param a + * @param d + * @param e + * @param i_e_start + * 演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる) + * @return + * @throws NyARException + */ + private void vecTridiagonalize(NyARVec d, NyARVec e,int i_e_start) throws NyARException + { + NyARVec vec=wk_vecTridiagonalize_vec; + //double[][] a_array=a.getArray(); + double s, t, p, q; + int dim; + + if(this.clm!=this.row){//if(a.getClm()!=a.getRow()){ + throw new NyARException(); + } + if(this.clm != d.getClm()){//if(a.getClm() != d.clm){ + throw new NyARException(); + } + if(this.clm != e.getClm()){//if(a.getClm() != e.clm){ + throw new NyARException(); + } + dim = this.getClm(); + + double[] d_vec,e_vec; + d_vec=d.getArray(); + e_vec=e.getArray(); + double[] a_vec_k; + + for(int k = 0; k < dim-2; k++ ){ + + a_vec_k=this.m[k]; + vec.setNewArray(a_vec_k,clm);//vec=this.getRowVec(k);//double[] vec_array=vec.getArray(); + NyARException.trap("未チェックパス"); + d_vec[k]=a_vec_k[k];//d.v[k]=vec.v[k];//d.set(k,v.get(k)); //d->v[k] = v[k]; + + //wv1.clm = dim-k-1; + //wv1.v = &(v[k+1]); + NyARException.trap("未チェックパス"); + e_vec[k+i_e_start]=vec.vecHousehold(k+1);//e.v[k+i_e_start]=vec.vecHousehold(k+1);//e->v[k] = arVecHousehold(&wv1); + if(e_vec[k+i_e_start]== 0.0 ){//if(e.v[k+i_e_start]== 0.0 ){//if(e.v[k+i_e_start]== 0.0 ){ + continue; + } + + for(int i = k+1; i < dim; i++ ){ + s = 0.0; + for(int j = k+1; j < i; j++ ) { + NyARException.trap("未チェックのパス"); + s += this.m[j][i] * a_vec_k[j];//s += a_array[j][i] * vec.v[j];//s += a.get(j*dim+i) * v.get(j);//s += a->m[j*dim+i] * v[j]; + } + for(int j = i; j < dim; j++ ) { + NyARException.trap("未チェックのパス"); + s += this.m[i][j] * a_vec_k[j];//s += a_array[i][j] * vec.v[j];//s += a.get(i*dim+j) * v.get(j);//s += a->m[i*dim+j] * v[j]; + } + NyARException.trap("未チェックのパス"); + d_vec[i]=s;//d.v[i]=s;//d->v[i] = s; + } + + + //wv1.clm = wv2.clm = dim-k-1; + //wv1.v = &(v[k+1]); + //wv2.v = &(d->v[k+1]); + a_vec_k=this.m[k]; + vec.setNewArray(a_vec_k,clm);//vec=this.getRowVec(k); +// vec_array=vec.getArray(); + NyARException.trap("未チェックパス"); + t = vec.vecInnerproduct(d,k+1)/ 2; + for(int i = dim-1; i > k; i-- ) { + NyARException.trap("未チェックパス"); + p = a_vec_k[i];//p = v.get(i);//p = v[i]; + d_vec[i]-=t*p;q=d_vec[i];//d.v[i]-=t*p;q=d.v[i];//q = d->v[i] -= t*p； + for(int j = i; j < dim; j++ ){ + NyARException.trap("未チェックパス"); + this.m[i][j]-=p*(d_vec[j] + q*a_vec_k[j]);//a.m[i][j]-=p*(d.v[j] + q*vec.v[j]);//a->m[i*dim+j] -= p*(d->v[j]) + q*v[j]; + } + } + } + + if( dim >= 2) { + d_vec[dim-2]=this.m[dim-2][dim-2];//d.v[dim-2]=a.m[dim-2][dim-2];//d->v[dim-2] = a->m[(dim-2)*dim+(dim-2)]; + e_vec[dim-2+i_e_start]=this.m[dim-2][dim-1];//e.v[dim-2+i_e_start]=a.m[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)]; + } + + if( dim >= 1 ){ + d_vec[dim-1]=this.m[dim-1][dim-1];//d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] = a->m[(dim-1)*dim+(dim-1)]; + } + NyARVec vec2=this.wk_vecTridiagonalize_vec2; + for(int k = dim-1; k >= 0; k--) { + a_vec_k=this.m[k]; + vec.setNewArray(a_vec_k,clm);//vec=this.getRowVec(k);//v = a.getPointer(k*dim);//v = &(a->m[k*dim]); + if( k < dim-2 ) { + for(int i = k+1; i < dim; i++ ){ + //wv1.clm = wv2.clm = dim-k-1; + //wv1.v = &(v[k+1]); + //wv2.v = &(a->m[i*dim+k+1]); + vec2.setNewArray(this.m[i],clm);//vec2=this.getRowVec(i); + + t = vec.vecInnerproduct(vec2,k+1); + for(int j = k+1; j < dim; j++ ){ + NyARException.trap("未チェックパス"); + this.m[i][j]-=t*a_vec_k[j];//a_array[i][j]-=t*vec.v[j];//a.subValue(i*dim+j,t*v.get(j));//a->m[i*dim+j] -= t * v[j]; + } + } + } + for(int i = 0; i < dim; i++ ){ + a_vec_k[i]=0.0;//v.set(i,0.0);//v[i] = 0.0; + } + a_vec_k[k]=1;//v.set(k,1);//v[k] = 1; + } + return; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/NyARParam.java b/src/jp/nyatla/nyartoolkit/core/NyARParam.java new file mode 100644 index 0000000..2fe7862 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARParam.java @@ -0,0 +1,480 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + + +import java.io.*; +import java.nio.*; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.util.DoubleValue; + +/*typedef struct { + int xsize, ysize; + double mat[3][4]; + double dist_factor[4]; +} ARParam;*/ +public class NyARParam{ + private static final int SIZE_OF_PARAM_SET=4+4+(3*4*8)+(4*8); + private static final int PD_LOOP = 3; + protected int xsize, ysize; + private double[] array34=new double[3*4];//Double2dArray mat=new Double2dArray(3,4); + private double[] dist_factor=new double[4]; + public int getX() + { + return xsize; + } + public int getY() + { + return ysize; + } + public double[] getDistFactor() + { + return dist_factor; + } + /** + * パラメタを格納した[4x3]配列を返します。 + * @return + */ + public final double[] get34Array() + { + return array34; + } + /** + * ARToolKit標準ファイルから1個目の設定をロードする。 + * @param i_filename + * @throws NyARException + */ + public void loadFromARFile(String i_filename) throws NyARException + { + try { + loadFromARFile(new FileInputStream(i_filename)); + } catch (Exception e) { + throw new NyARException(e); + } + } + public void loadFromARFile(InputStream i_stream) throws NyARException + { + try { + NyARParam new_inst[] = arParamLoad(i_stream, 1); + i_stream.close(); + xsize = new_inst[0].xsize; + ysize = new_inst[0].ysize; + array34 = new_inst[0].array34; + dist_factor = new_inst[0].dist_factor; + } catch (Exception e) { + throw new NyARException(e); + } + } + /*static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )*/ + private final static double dot( double a1, double a2, double a3,double b1, double b2, double b3 ) + { + return( a1 * b1 + a2 * b2 + a3 * b3 ); + } + /* static double norm( double a, double b, double c )*/ + private final static double norm( double a, double b, double c ) + { + return Math.sqrt( a*a + b*b + c*c ); + } + /** + * int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] ); + * 関数の置き換え + * Optimize STEP[754->665] + * @param o_cpara + * 戻り引数。3x4のマトリクスを指定すること。 + * @param o_trans + * 戻り引数。3x4のマトリクスを指定すること。 + * @return + */ + public void decompMat(NyARMat o_cpara, NyARMat o_trans) + { + double[] source=array34; + double[] Cpara=new double[3*4];//double Cpara[3][4]; + double rem1, rem2, rem3; + int i; + if(source[2*4+3]>= 0 ){//if( source[2][3] >= 0 ) { + // + //for(int r = 0; r < 3; r++ ){ + // for(int c = 0; c < 4; c++ ){ + // Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c]; + // } + //} + for(i=0;i<12;i++){ + Cpara[i]=source[i];//Cpara[r][c] = source[r][c]; + } + // + }else { + // + //for(int r = 0; r < 3; r++ ){ + // for(int c = 0; c < 4; c++ ){ + // Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]); + // } + //} + for(i=0;i<12;i++){ + Cpara[i]=source[i];//Cpara[r][c] = source[r][c]; + } + // + } + + double[][] cpara=o_cpara.getArray(); + double[][] trans=o_trans.getArray(); + for(int r = 0; r < 3; r++ ){ + for(int c = 0; c < 4; c++ ){ + cpara[r][c]=0.0;//cpara[r][c] = 0.0; + } + } + cpara[2][2]=norm(Cpara[2*4+0],Cpara[2*4+1], Cpara[2*4+2]);//cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] ); + trans[2][0]=Cpara[2*4+0] / cpara[2][2];//trans[2][0] = Cpara[2][0] / cpara[2][2]; + trans[2][1]=Cpara[2*4+1]/ cpara[2][2];//trans[2][1] = Cpara[2][1] / cpara[2][2]; + trans[2][2]=Cpara[2*4+2]/ cpara[2][2];//trans[2][2] = Cpara[2][2] / cpara[2][2]; + trans[2][3]=Cpara[2*4+3] / cpara[2][2];//trans[2][3] = Cpara[2][3] / cpara[2][2]; + + cpara[1][2]=dot(trans[2][0], trans[2][1], trans[2][2],Cpara[1*4+0], Cpara[1*4+1], Cpara[1*4+2]);//cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2] ); + rem1=Cpara[1*4+0]- cpara[1][2] * trans[2][0];//rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0]; + rem2=Cpara[1*4+1] - cpara[1][2] * trans[2][1];//rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1]; + rem3=Cpara[1*4+2] - cpara[1][2] * trans[2][2];//rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2]; + cpara[1][1]=norm(rem1, rem2, rem3 );//cpara[1][1] = norm( rem1, rem2, rem3 ); + trans[1][0]= rem1/cpara[1][1];//trans[1][0] = rem1 / cpara[1][1]; + trans[1][1]=rem2/cpara[1][1];//trans[1][1] = rem2 / cpara[1][1]; + trans[1][2]=rem3 / cpara[1][1];//trans[1][2] = rem3 / cpara[1][1]; + + cpara[0][2]=dot(trans[2][0],trans[2][1], trans[2][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2] );//cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); + cpara[0][1]=dot(trans[1][0],trans[1][1], trans[1][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2]);//cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] ); + rem1=Cpara[0*4+0]- cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];//rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0]; + rem2 = Cpara[0*4+1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];//rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1]; + rem3 = Cpara[0*4+2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];//rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2]; + cpara[0][0]=norm(rem1, rem2, rem3);//cpara[0][0] = norm( rem1, rem2, rem3 ); + trans[0][0]=rem1 / cpara[0][0];//trans[0][0] = rem1 / cpara[0][0]; + trans[0][1]= rem2 / cpara[0][0];//trans[0][1] = rem2 / cpara[0][0]; + trans[0][2]= rem3 / cpara[0][0];//trans[0][2] = rem3 / cpara[0][0]; + + trans[1][3]=(Cpara[1*4+3] - cpara[1][2]*trans[2][3]) / cpara[1][1];//trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1]; + trans[0][3]=(Cpara[0*4+3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0];//trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0]; + + for(int r = 0; r < 3; r++ ){ + for(int c = 0; c < 3; c++ ){ + cpara[r][c]/=cpara[2][2];//cpara[r][c] /= cpara[2][2]; + } + } + } + + + /*int arParamDisp( ARParam *param );*/ + public int paramDisp() + { + System.out.println("--------------------------------------");//printf("--------------------------------------\n"); + System.out.print("SIZE = "+xsize+", "+ysize);//printf("SIZE = %d, %d\n", param->xsize, param->ysize); + System.out.println("Distortion factor = "+dist_factor[0]+" "+dist_factor[1]+" "+dist_factor[2]+" "+dist_factor[3]);//printf("Distortion factor = %f %f %f %f\n", param->dist_factor[0],param->dist_factor[1], param->dist_factor[2], param->dist_factor[3] ); + for(int j = 0; j < 3; j++ ) {//for(j = 0; j < 3; j++ ) { + for(int i = 0; i < 4; i++ ){ + System.out.print(array34[j*4+i]+" ");//printf("%7.5f ", param->mat[j][i]); + } + System.out.println();// printf("\n"); + }//} + System.out.println("--------------------------------------");//printf("--------------------------------------\n"); + return 0; + } +// /*int arParamDecomp( ARParam *source, ARParam *icpara, double trans[3][4] );*/ +// private static int arParamDecomp( NyARParam source, NyARParam icpara, double[][] trans) +// { +// icpara.xsize = source.xsize;//icpara->xsize = source->xsize; +// icpara.ysize = source.ysize;//icpara->ysize = source->ysize; +// icpara.dist_factor[0] = source.dist_factor[0];//icpara->dist_factor[0] = source->dist_factor[0]; +// icpara.dist_factor[1] = source.dist_factor[1];// icpara->dist_factor[1] = source->dist_factor[1]; +// icpara.dist_factor[2] = source.dist_factor[2];//icpara->dist_factor[2] = source->dist_factor[2]; +// icpara.dist_factor[3] = source.dist_factor[3];//icpara->dist_factor[3] = source->dist_factor[3]; +// return arParamDecompMat(source.mat, icpara.mat, trans ); +// } + /** + * int arParamChangeSize( ARParam *source, int xsize, int ysize, ARParam *newparam ); + * 関数の代替関数 + * サイズプロパティをi_xsize,i_ysizeに変更します。 + * @param xsize + * @param ysize + * @param newparam + * @return + * + */ + public void changeSize(int i_xsize, int i_ysize) + { + double scale; + scale = (double)i_xsize / (double)(xsize);//scale = (double)xsize / (double)(source->xsize); + + for(int i = 0; i < 4; i++ ) { + array34[0*4+i]=array34[0*4+i]*scale;//newparam->mat[0][i] = source->mat[0][i] * scale; + array34[1*4+i]=array34[1*4+i]*scale;//newparam->mat[1][i] = source->mat[1][i] * scale; + array34[2*4+i]=array34[2*4+i];//newparam->mat[2][i] = source->mat[2][i]; + } + + dist_factor[0] = dist_factor[0] * scale;//newparam->dist_factor[0] = source->dist_factor[0] * scale; + dist_factor[1] = dist_factor[1] * scale;//newparam->dist_factor[1] = source->dist_factor[1] * scale; + dist_factor[2] = dist_factor[2] / (scale*scale);//newparam->dist_factor[2] = source->dist_factor[2] / (scale*scale); + dist_factor[3] = dist_factor[3];//newparam->dist_factor[3] = source->dist_factor[3]; + + xsize = i_xsize;//newparam->xsize = xsize; + ysize = i_ysize;//newparam->ysize = ysize; + } + /** + * int arParamIdeal2Observ( const double dist_factor[4], const double ix, const double iy,double *ox, double *oy ) + * 関数の代替関数 + * @param ix + * @param iy + * @param ox + * @param oy + */ + public void ideal2Observ(double ix,double iy,DoubleValue ox, DoubleValue oy) + { + + double x, y, d; + final double d0,d1,d3; + final double df[]=this.dist_factor; + d0=df[0]; + d1=df[1]; + d3=df[3]; + x = (ix - d0) * d3; + y = (iy - d1) * d3; + if( x == 0.0 && y == 0.0 ) { + ox.value=d0; + oy.value=d1; + }else{ + d = 1.0 - df[2]/100000000.0 * (x*x+y*y); + ox.value=x * d + d0; + oy.value=y * d + d1; + } + } + /** + * ideal2Observをまとめて実行します。 + * @param i_in + * double[][2] + * @param o_out + * double[][2] + */ + public void ideal2ObservBatch(double[][] i_in,double[][] o_out,int i_size) + { + + double x, y, d; + final double d0,d1,d3,d2_w; + final double df[]=this.dist_factor; + d0=df[0]; + d1=df[1]; + d3=df[3]; + d2_w=df[2]/100000000.0; + for(int i=0;i + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +/** + * ARMarkerInfoに相当するクラス。 スクエア情報を保持します。 + * + */ +public class NyARSquare extends NyARMarker { + // private NyARMarker marker; + // public int area; + // public double[] pos; + public double[][] line = new double[4][3]; // double[4][3] + public double[][] sqvertex = new double[4][2];// double[4][2]; + + public NyARSquare() { + super(); + } + + private final NyARMat wk_getLine_input = new NyARMat(1, 2); + private final NyARMat wk_getLine_evec = new NyARMat(2, 2); + private final NyARVec wk_getLine_ev = new NyARVec(2); + private final NyARVec wk_getLine_mean = new NyARVec(2); + + /** + * arGetLine(int x_coord[], int y_coord[], int coord_num,int vertex[], + * double line[4][3], double v[4][2]) arGetLine2(int x_coord[], int + * y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2], + * double *dist_factor) の２関数の合成品です。 格納しているマーカー情報に対して、GetLineの計算を行い、結果を返します。 + * Optimize:STEP[424->391] + * + * @param i_cparam + * @return + * @throws NyARException + */ + public boolean getLine(NyARParam i_cparam) throws NyARException { + double w1; + int st, ed, n; + int i; + + final double[][] l_sqvertex = this.sqvertex; + final double[][] l_line = this.line; + final int[] l_mkvertex = this.mkvertex; + final int[] l_x_coord = this.x_coord; + final int[] l_y_coord = this.y_coord; + final NyARVec ev = this.wk_getLine_ev; // matrixPCAの戻り値を受け取る + final NyARVec mean = this.wk_getLine_mean;// matrixPCAの戻り値を受け取る + final double[] mean_array = mean.getArray(); + double[] l_line_i, l_line_2; + + NyARMat input = this.wk_getLine_input;// 次処理で初期化される。 + NyARMat evec = this.wk_getLine_evec;// アウトパラメータを受け取るから初期化不要//new + // NyARMat(2,2); + double[][] evec_array = evec.getArray(); + for (i = 0; i < 4; i++) { + w1 = (double) (l_mkvertex[i + 1] - l_mkvertex[i] + 1) * 0.05 + 0.5; + st = (int) (l_mkvertex[i] + w1); + ed = (int) (l_mkvertex[i + 1] - w1); + n = ed - st + 1; + if (n < 2) { + // nが2以下でmatrix.PCAを計算することはできないので、エラーにしておく。 + return false;// throw new NyARException(); + } + input.realloc(n, 2); + // バッチ取得 + i_cparam.observ2IdealBatch(l_x_coord, l_y_coord, st, n, input + .getArray()); + // for( j = 0; j < n; j++ ) { + // i_cparam.observ2Ideal(l_x_coord[st+j], + // l_y_coord[st+j],dv1,dv2);//arParamObserv2Ideal( dist_factor, + // x_coord[st+j], y_coord[st+j],&(input->m[j*2+0]), + // &(input->m[j*2+1]) ); + // in_array[j][0]=dv1.value; + // in_array[j][1]=dv2.value; + // } + input.matrixPCA(evec, ev, mean); + l_line_i = l_line[i]; + l_line_i[0] = evec_array[0][1];// line[i][0] = evec->m[1]; + l_line_i[1] = -evec_array[0][0];// line[i][1] = -evec->m[0]; + l_line_i[2] = -(l_line_i[0] * mean_array[0] + l_line_i[1] + * mean_array[1]);// line[i][2] = -(line[i][0]*mean->v[0] + // + line[i][1]*mean->v[1]); + } + + for (i = 0; i < 4; i++) { + l_line_i = l_line[i]; + l_line_2 = l_line[(i + 3) % 4]; + w1 = l_line_2[0] * l_line_i[1] - l_line_i[0] * l_line_2[1]; + if (w1 == 0.0) { + return false; + } + l_sqvertex[i][0] = (l_line_2[1] * l_line_i[2] - l_line_i[1] + * l_line_2[2]) + / w1; + l_sqvertex[i][1] = (l_line_i[0] * l_line_2[2] - l_line_2[0] + * l_line_i[2]) + / w1; + } + return true; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARSquareList.java b/src/jp/nyatla/nyartoolkit/core/NyARSquareList.java new file mode 100644 index 0000000..638c302 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARSquareList.java @@ -0,0 +1,70 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +public class NyARSquareList extends NyARMarkerList +{ + private final NyARSquare[] square_array; + private int square_array_num; + public NyARSquareList(int i_number_of_holder) + { + super(new NyARSquare[i_number_of_holder]); + //マーカーホルダに実体を割り当てる。 + for(int i=0;i=this.square_array_num){ + throw new NyARException(); + } + return this.square_array[i_index]; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARTransMat.java b/src/jp/nyatla/nyartoolkit/core/NyARTransMat.java new file mode 100644 index 0000000..e813de8 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARTransMat.java @@ -0,0 +1,50 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + + + + +import jp.nyatla.nyartoolkit.*; + + +/** + * This class calculates ARMatrix from square information. + * -- + * 変換行列を計算するクラス。 + * + */ +public interface NyARTransMat{ + public void setCenter(double i_x,double i_y); + public double transMat(NyARSquare i_square,int i_direction, double i_width,NyARTransMatResult o_result)throws NyARException; + public double transMatContinue(NyARSquare i_square,int i_direction, double i_width,NyARTransMatResult io_result_conv)throws NyARException; +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARTransMatResult.java b/src/jp/nyatla/nyartoolkit/core/NyARTransMatResult.java new file mode 100644 index 0000000..b0c89ec --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARTransMatResult.java @@ -0,0 +1,93 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.*; +/** + * NyARTransMat戻り値専用のNyARMat + * + */ +public class NyARTransMatResult extends NyARMat +{ + private boolean has_value=false; + public NyARTransMatResult() + { + super(3,4); + } + /** + * この関数は使えません。 + * @param i_row + * @param i_clm + * @throws NyARException + */ + public NyARTransMatResult(int i_row,int i_clm) throws NyARException + { + super();//ここで例外発生 + } + /** + * パラメータで変換行列を更新します。 + * @param i_rot + * @param i_off + * @param i_trans + */ + public void updateMatrixValue(NyARTransRot i_rot,double[] i_off,double[] i_trans) + { + double[] pa; + double[] rot=i_rot.getArray(); + + pa=this.m[0]; + pa[0] = rot[0*3+0]; + pa[1] = rot[0*3+1]; + pa[2] = rot[0*3+2]; + pa[3] = rot[0*3+0]*i_off[0] + rot[0*3+1]*i_off[1] + rot[0*3+2]*i_off[2] + i_trans[0]; + + pa=this.m[1]; + pa[0] = rot[1*3+0]; + pa[1] = rot[1*3+1]; + pa[2] = rot[1*3+2]; + pa[3] = rot[1*3+0]*i_off[0] + rot[1*3+1]*i_off[1] + rot[1*3+2]*i_off[2] + i_trans[1]; + + pa=this.m[2]; + pa[0] = rot[2*3+0]; + pa[1] = rot[2*3+1]; + pa[2] = rot[2*3+2]; + pa[3] = rot[2*3+0]*i_off[0] + rot[2*3+1]*i_off[1] + rot[2*3+2]*i_off[2] + i_trans[2]; + + + this.has_value=true; + return; + } + public void copyFrom(NyARTransMatResult i_from) throws NyARException + { + super.copyFrom(i_from); + this.has_value=i_from.has_value; + } + public boolean hasValue() + { + return this.has_value; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARTransMat_O1.java b/src/jp/nyatla/nyartoolkit/core/NyARTransMat_O1.java new file mode 100644 index 0000000..0d3abbf --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARTransMat_O1.java @@ -0,0 +1,543 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.util.DoubleValue; + +public class NyARTransMat_O1 implements NyARTransMat { + private final static int AR_FITTING_TO_IDEAL = 0;// #define + // AR_FITTING_TO_IDEAL 0 + private final static int AR_FITTING_TO_INPUT = 1;// #define + // AR_FITTING_TO_INPUT 1 + private final static int arFittingMode = AR_FITTING_TO_INPUT; + + private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define + // AR_GET_TRANS_MAT_MAX_LOOP_COUNT + // 5 + private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define + // AR_GET_TRANS_MAT_MAX_FIT_ERROR + // 1.0 + private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0; + private final static int P_MAX = 10;// 頂点の数(4で十分だけどなんとなく10)//#define P_MAX + // 500 + private final static int NUMBER_OF_VERTEX = 4;// 処理対象の頂点数 + private final NyARTransRot transrot; + private final double[] center = { 0.0, 0.0 }; + private final NyARParam param; + + // private final NyARMat result_mat=new NyARMat(3,4); + public NyARTransMat_O1(NyARParam i_param) throws NyARException { + param = i_param; + transrot = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX); + + } + + public void setCenter(double i_x, double i_y) { + center[0] = i_x; + center[1] = i_x; + } + + // public NyARMat getTransformationMatrix() + // { + // return result_mat; + // } + + private final double[][] wk_transMat_pos3d = new double[P_MAX][3];// pos3d[P_MAX][3]; + private final double[][] wk_transMat_ppos2d = new double[4][2]; + private final double[][] wk_transMat_ppos3d = new double[4][2]; + private final double[] wk_transMat_off = new double[3]; + + /** + * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double + * width, double conv[3][4] ) 関数の置き換え。 保持している変換行列を更新する。 + * + * @param square + * 計算対象のNyARSquareオブジェクト + * @param i_direction + * マーカーの方向 + * @param i_width + * マーカーのサイズ(mm) + * @param o_result_conv + * 変換行列を受け取るオブジェクトを指定します。 + * @return + * @throws NyARException + */ + public double transMat(NyARSquare square, int i_direction, double i_width, + NyARTransMatResult o_result_conv) throws NyARException { + double[][] ppos2d = wk_transMat_ppos2d; + double[][] ppos3d = wk_transMat_ppos3d; + double[] off = wk_transMat_off; + double[][] pos3d = wk_transMat_pos3d; + int dir; + double err = -1; + + transrot.initRot(square, i_direction); + + dir = i_direction; + ppos2d[0][0] = square.sqvertex[(4 - dir) % 4][0]; + ppos2d[0][1] = square.sqvertex[(4 - dir) % 4][1]; + ppos2d[1][0] = square.sqvertex[(5 - dir) % 4][0]; + ppos2d[1][1] = square.sqvertex[(5 - dir) % 4][1]; + ppos2d[2][0] = square.sqvertex[(6 - dir) % 4][0]; + ppos2d[2][1] = square.sqvertex[(6 - dir) % 4][1]; + ppos2d[3][0] = square.sqvertex[(7 - dir) % 4][0]; + ppos2d[3][1] = square.sqvertex[(7 - dir) % 4][1]; + ppos3d[0][0] = center[0] - i_width / 2.0; + ppos3d[0][1] = center[1] + i_width / 2.0; + ppos3d[1][0] = center[0] + i_width / 2.0; + ppos3d[1][1] = center[1] + i_width / 2.0; + ppos3d[2][0] = center[0] + i_width / 2.0; + ppos3d[2][1] = center[1] - i_width / 2.0; + ppos3d[3][0] = center[0] - i_width / 2.0; + ppos3d[3][1] = center[1] - i_width / 2.0; + + // arGetTransMat3の前段処理(pos3dとoffを初期化) + arGetTransMat3_initPos3d(ppos3d, pos3d, off); + + for (int i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { + err = arGetTransMat3(ppos2d, pos3d, off, o_result_conv); + if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { + break; + } + } + + return err; + } + + /** + * transMatContinue用のワーク + */ + private final NyARTransMatResult wk_transMatContinue_result = new NyARTransMatResult(); + + /** + * double arGetTransMatCont( ARMarkerInfo *marker_info, double + * prev_conv[3][4],double center[2], double width, double conv[3][4] ) + * + * @param i_square + * @param i_direction + * マーカーの方位を指定する。 + * @param i_width + * @param io_result_conv + * 計算履歴を持つNyARTransMatResultオブジェクトを指定する。 + * 履歴を持たない場合は、transMatと同じ処理を行う。 + * @return + * @throws NyARException + */ + public double transMatContinue(NyARSquare i_square, int i_direction, + double i_width, NyARTransMatResult io_result_conv) + throws NyARException { + // io_result_convが初期値なら、transMatで計算する。 + if (!io_result_conv.hasValue()) { + return this + .transMat(i_square, i_direction, i_width, io_result_conv); + } + + double err1, err2; + int i, dir; + double[][] ppos2d = wk_transMat_ppos2d; + double[][] ppos3d = wk_transMat_ppos3d; + double[] off = wk_transMat_off; + double[][] pos3d = wk_transMat_pos3d; + + // arGetTransMatContSub計算部分 + transrot.initRotByPrevResult(io_result_conv); + + dir = i_direction; + ppos2d[0][0] = i_square.sqvertex[(4 - dir) % 4][0]; + ppos2d[0][1] = i_square.sqvertex[(4 - dir) % 4][1]; + ppos2d[1][0] = i_square.sqvertex[(5 - dir) % 4][0]; + ppos2d[1][1] = i_square.sqvertex[(5 - dir) % 4][1]; + ppos2d[2][0] = i_square.sqvertex[(6 - dir) % 4][0]; + ppos2d[2][1] = i_square.sqvertex[(6 - dir) % 4][1]; + ppos2d[3][0] = i_square.sqvertex[(7 - dir) % 4][0]; + ppos2d[3][1] = i_square.sqvertex[(7 - dir) % 4][1]; + ppos3d[0][0] = center[0] - i_width / 2.0; + ppos3d[0][1] = center[1] + i_width / 2.0; + ppos3d[1][0] = center[0] + i_width / 2.0; + ppos3d[1][1] = center[1] + i_width / 2.0; + ppos3d[2][0] = center[0] + i_width / 2.0; + ppos3d[2][1] = center[1] - i_width / 2.0; + ppos3d[3][0] = center[0] - i_width / 2.0; + ppos3d[3][1] = center[1] - i_width / 2.0; + + // arGetTransMat3の前段処理(pos3dとoffを初期化) + arGetTransMat3_initPos3d(ppos3d, pos3d, off); + + err1 = err2 = -1; + for (i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { + err1 = arGetTransMat3(ppos2d, pos3d, off, io_result_conv); + if (err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { + // 十分な精度を達成できたらブレーク + break; + } + } + + // エラー値が許容範囲でなければTransMatをやり直し + if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) { + NyARTransMatResult result2 = this.wk_transMatContinue_result; + // transMatを実行 + transrot.initRot(i_square, i_direction); + err2 = transMat(i_square, i_direction, i_width, result2); + // transmMatここまで + if (err2 < err1) { + io_result_conv.copyFrom(result2); + err1 = err2; + } + } + return err1; + } + + private final double[] wk_arGetTransMat3_initPos3d_pmax = new double[3]; + private final double[] wk_arGetTransMat3_initPos3d_pmin = new double[3]; + + /** + * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。 + * 計算結果から再帰的に変更される可能性が無いので、切り離し。 + * + * @param i_ppos3d + * 入力配列[num][3] + * @param o_pos3d + * 出力配列[P_MAX][3] + * @param o_off + * [3] + * @throws NyARException + */ + private final void arGetTransMat3_initPos3d(double i_ppos3d[][], + double[][] o_pos3d, double[] o_off) throws NyARException { + double[] pmax = wk_arGetTransMat3_initPos3d_pmax;// new double[3]; + double[] pmin = wk_arGetTransMat3_initPos3d_pmin;// new + // double[3];//double + // off[3], pmax[3], + // pmin[3]; + int i; + pmax[0] = pmax[1] = pmax[2] = -10000000000.0; + pmin[0] = pmin[1] = pmin[2] = 10000000000.0; + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + if (i_ppos3d[i][0] > pmax[0]) { + pmax[0] = i_ppos3d[i][0]; + } + if (i_ppos3d[i][0] < pmin[0]) { + pmin[0] = i_ppos3d[i][0]; + } + if (i_ppos3d[i][1] > pmax[1]) { + pmax[1] = i_ppos3d[i][1]; + } + if (i_ppos3d[i][1] < pmin[1]) { + pmin[1] = i_ppos3d[i][1]; + } + /* + * オリジナルでもコメントアウト if( ppos3d[i][2] > pmax[2] ) pmax[2] = + * ppos3d[i][2]; if( ppos3d[i][2] < pmin[2] ) pmin[2] = + * ppos3d[i][2]; + */ + } + o_off[0] = -(pmax[0] + pmin[0]) / 2.0; + o_off[1] = -(pmax[1] + pmin[1]) / 2.0; + o_off[2] = -(pmax[2] + pmin[2]) / 2.0; + + double[] o_pos3d_pt; + double[] i_pos_pd_pt; + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + o_pos3d_pt = o_pos3d[i]; + i_pos_pd_pt = i_ppos3d[i]; + o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0]; + o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1]; + o_pos3d_pt[2] = 0.0; + } + } + + /** + * double arGetTransMat3( double rot[3][3], double ppos2d[][2],double + * ppos3d[][2], int num, double conv[3][4],double *dist_factor, double + * cpara[3][4] ) STEP 414-> + * + * @param ppos2d + * @param i_pos3d + * @param i_off + * @param num + * @return + * @throws NyARException + */ + private final double arGetTransMat3(double ppos2d[][], + final double i_pos3d[][], final double i_off[], + NyARTransMatResult o_result_conv) throws NyARException { + + double ret; + ret = arGetTransMatSub(ppos2d, i_pos3d, i_off, o_result_conv); + + // double[][] conv=o_result_conv.getArray(); + // double[] rot=transrot.getArray(); + // for(int i=0;i<3;i++){ + // conv[i][0] = rot[i*3+0]; + // conv[i][1] = rot[i*3+1]; + // conv[i][2] = rot[i*3+2]; + // } + // conv[0][3] = conv[0][0]*i_off[0] + conv[0][1]*i_off[1] + + // conv[0][2]*i_off[2] + conv[0][3]; + // conv[1][3] = conv[1][0]*i_off[0] + conv[1][1]*i_off[1] + + // conv[1][2]*i_off[2] + conv[1][3]; + // conv[2][3] = conv[2][0]*i_off[0] + conv[2][1]*i_off[1] + + // conv[2][2]*i_off[2] + conv[2][3]; + return ret; + } + + private final NyARMat wk_arGetTransMatSub_mat_a = new NyARMat( + NUMBER_OF_VERTEX * 2, 3); + private final NyARMat wk_arGetTransMatSub_mat_b = new NyARMat(3, + NUMBER_OF_VERTEX * 2); + private final NyARMat wk_arGetTransMatSub_mat_c = new NyARMat( + NUMBER_OF_VERTEX * 2, 1); + private final NyARMat wk_arGetTransMatSub_mat_d = new NyARMat(3, 3); + private final NyARMat wk_arGetTransMatSub_mat_e = new NyARMat(3, 1); + private final NyARMat wk_arGetTransMatSub_mat_f = new NyARMat(3, 1); + private final double[] wk_arGetTransMatSub_trans = new double[3]; + private final double[][] wk_arGetTransMatSub_pos2d = new double[P_MAX][2];// pos2d[P_MAX][2]; + private final DoubleValue wk_arGetTransMatSub_a1 = new DoubleValue(); + private final DoubleValue wk_arGetTransMatSub_a2 = new DoubleValue(); + + /** + * static double arGetTransMatSub( double rot[3][3], double + * ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double + * *dist_factor, double cpara[3][4] ) Optimize:2008.04.20:STEP[1033→1004] + * + * @param i_ppos2d + * @param i_pos3d + * @return + * @throws NyARException + */ + private final double arGetTransMatSub(double i_ppos2d[][], + double i_pos3d[][], double[] i_off, NyARTransMatResult o_result_conv) + throws NyARException { + double[][] pos2d = wk_arGetTransMatSub_pos2d; + double cpara[] = param.get34Array(); + NyARMat mat_a, mat_b, mat_c, mat_d, mat_e, mat_f;// ARMat *mat_a, + // *mat_b, *mat_c, + // *mat_d, *mat_e, + // *mat_f; + + double wx, wy, wz; + double ret; + int i; + double[] po2d_pt; + + mat_a = this.wk_arGetTransMatSub_mat_a; + // mat_a.realloc(NUMBER_OF_VERTEX*2,3); + double[][] a_array = mat_a.getArray(); + + mat_b = this.wk_arGetTransMatSub_mat_b; + // mat_b.realloc(3,NUMBER_OF_VERTEX*2); + double[][] b_array = mat_b.getArray(); + + DoubleValue a1 = wk_arGetTransMatSub_a1; + DoubleValue a2 = wk_arGetTransMatSub_a2; + if (arFittingMode == AR_FITTING_TO_INPUT) { + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + param.ideal2Observ(i_ppos2d[i][0], i_ppos2d[i][1], a1, a2);// arParamIdeal2Observ(dist_factor, + // ppos2d[i][0], + // ppos2d[i][1],&pos2d[i][0], + // &pos2d[i][1]); + po2d_pt = pos2d[i]; + po2d_pt[0] = a1.value; + po2d_pt[1] = a2.value; + } + } else { + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + po2d_pt = pos2d[i]; + pos2d[i][0] = i_ppos2d[i][0]; + pos2d[i][1] = i_ppos2d[i][1]; + } + } + mat_c = this.wk_arGetTransMatSub_mat_c;// 次処理で値をもらうので、初期化の必要は無い。 + // mat_c.realloc(NUMBER_OF_VERTEX*2,1); + double[][] c_array = mat_c.getArray(); + double[] rot = transrot.getArray(); + double[] i_pos3d_pt; + int x2; + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + x2 = i * 2; + i_pos3d_pt = i_pos3d[i]; + po2d_pt = pos2d[i]; + wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] + * i_pos3d_pt[2]; + wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] + * i_pos3d_pt[2]; + wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] + * i_pos3d_pt[2]; + // + a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0] + // = + // mat_b->m[num*0+j*2] + // = + // cpara[0][0]; + a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1] + // = + // mat_b->m[num*2+j*2] + // = + // cpara[0][1]; + a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - po2d_pt[0];// mat_a->m[j*6+2] + // = + // mat_b->m[num*4+j*2] + // = + // cpara[0][2] + // - + // pos2d[j][0]; + a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] = + // mat_b->m[num*0+j*2+1] + // = 0.0; + a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4] + // = + // mat_b->m[num*2+j*2+1] + // = + // cpara[1][1]; + a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] + - po2d_pt[1];// mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = + // cpara[1][2] - pos2d[j][1]; + c_array[x2][0] = wz * po2d_pt[0] - cpara[0 * 4 + 0] * wx + - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] + // = wz * + // pos2d[j][0]- + // cpara[0][0]*wx + // - + // cpara[0][1]*wy + // - + // cpara[0][2]*wz; + c_array[x2 + 1][0] = wz * po2d_pt[1] - cpara[1 * 4 + 1] * wy + - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz * + // pos2d[j][1]- cpara[1][1]*wy - + // cpara[1][2]*wz; + } + mat_d = this.wk_arGetTransMatSub_mat_d;// 次処理で値をもらうので、初期化の必要は無い。 + mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。 + mat_f = this.wk_arGetTransMatSub_mat_f;// 次処理で値をもらうので、初期化の必要は無い。 + double[][] f_array = mat_f.getArray(); + + mat_d.matrixMul(mat_b, mat_a); + mat_e.matrixMul(mat_b, mat_c); + mat_d.matrixSelfInv(); + mat_f.matrixMul(mat_d, mat_e); + + double[] trans = wk_arGetTransMatSub_trans;// double trans[3]; + trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; + trans[1] = f_array[1][0]; + trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; + + ret = transrot.modifyMatrix(trans, i_pos3d, pos2d); + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + x2 = i * 2; + i_pos3d_pt = i_pos3d[i]; + po2d_pt = pos2d[i]; + // + // wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] + // * pos3d[j][2]; + // wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] + // * pos3d[j][2]; + // wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] + // * pos3d[j][2]; + wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] + * i_pos3d_pt[2]; + wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] + * i_pos3d_pt[2]; + wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] + * i_pos3d_pt[2]; + // + a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0] + // = + // mat_b->m[num*0+j*2] + // = + // cpara[0][0]; + a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1] + // = + // mat_b->m[num*2+j*2] + // = + // cpara[0][1]; + a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - po2d_pt[0];// mat_a->m[j*6+2] + // = + // mat_b->m[num*4+j*2] + // = + // cpara[0][2] + // - + // pos2d[j][0]; + a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] = + // mat_b->m[num*0+j*2+1] + // = 0.0; + a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4] + // = + // mat_b->m[num*2+j*2+1] + // = + // cpara[1][1]; + a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] + - po2d_pt[1];// mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = + // cpara[1][2] - pos2d[j][1]; + c_array[x2][0] = wz * po2d_pt[0] - cpara[0 * 4 + 0] * wx + - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] + // = wz * + // pos2d[j][0]- + // cpara[0][0]*wx + // - + // cpara[0][1]*wy + // - + // cpara[0][2]*wz; + c_array[x2 + 1][0] = wz * po2d_pt[1] - cpara[1 * 4 + 1] * wy + - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz * + // pos2d[j][1]- cpara[1][1]*wy - + // cpara[1][2]*wz; + } + + mat_d.matrixMul(mat_b, mat_a); + mat_e.matrixMul(mat_b, mat_c); + mat_d.matrixSelfInv(); + mat_f.matrixMul(mat_d, mat_e); + + trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; + trans[1] = f_array[1][0]; + trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; + + ret = transrot.modifyMatrix(trans, i_pos3d, pos2d); + // 変換行列を保存 + o_result_conv.updateMatrixValue(this.transrot, i_off, trans); + + // double[][] conv=o_result_conv.getArray(); + // for( i = 2; i >=0; i-- ) {//for( j = 0; j < 3; j++ ) { + // // + // //for( i = 0; i < 3; i++ ){ + // // conv[j][i] = rot[j][i]; + // //} + // conv[i][0] = rot[i*3+0]; + // conv[i][1] = rot[i*3+1]; + // conv[i][2] = rot[i*3+2]; + // // + // conv[i][3] = trans[i]; + // } + return ret; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARTransMat_O2.java b/src/jp/nyatla/nyartoolkit/core/NyARTransMat_O2.java new file mode 100644 index 0000000..f45e23e --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARTransMat_O2.java @@ -0,0 +1,465 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +/** + * This class calculates ARMatrix from square information and holds it. -- + * 変換行列を計算して、結果を保持するクラス。 + * + */ +public class NyARTransMat_O2 implements NyARTransMat { + private final static int AR_FITTING_TO_IDEAL = 0;// #define + // AR_FITTING_TO_IDEAL 0 + private final static int AR_FITTING_TO_INPUT = 1;// #define + // AR_FITTING_TO_INPUT 1 + private final static int arFittingMode = AR_FITTING_TO_INPUT; + + private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define + // AR_GET_TRANS_MAT_MAX_LOOP_COUNT + // 5 + private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define + // AR_GET_TRANS_MAT_MAX_FIT_ERROR + // 1.0 + private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0; + private final static int P_MAX = 10;// 頂点の数(4で十分だけどなんとなく10)//#define P_MAX + // 500 + private final static int NUMBER_OF_VERTEX = 4;// 処理対象の頂点数 + private final NyARTransRot transrot; + private final double[] center = { 0.0, 0.0 }; + private final NyARParam param; + private final NyARMat result_mat = new NyARMat(3, 4); + + public NyARTransMat_O2(NyARParam i_param) throws NyARException { + param = i_param; + transrot = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX); + + } + + public void setCenter(double i_x, double i_y) { + center[0] = i_x; + center[1] = i_x; + } + + public NyARMat getTransformationMatrix() { + return result_mat; + } + + /** + * transMat関数の初期化関数を分離したものです。 + * + * @param square + * @param i_direction + * @param i_width + * @param o_ppos2d + * @param o_ppos3d + */ + private final void init_transMat_ppos(NyARSquare square, int i_direction, + double i_width, double[][] o_ppos2d, double[][] o_ppos3d) { + o_ppos2d[0][0] = square.sqvertex[(4 - i_direction) % 4][0]; + o_ppos2d[0][1] = square.sqvertex[(4 - i_direction) % 4][1]; + o_ppos2d[1][0] = square.sqvertex[(5 - i_direction) % 4][0]; + o_ppos2d[1][1] = square.sqvertex[(5 - i_direction) % 4][1]; + o_ppos2d[2][0] = square.sqvertex[(6 - i_direction) % 4][0]; + o_ppos2d[2][1] = square.sqvertex[(6 - i_direction) % 4][1]; + o_ppos2d[3][0] = square.sqvertex[(7 - i_direction) % 4][0]; + o_ppos2d[3][1] = square.sqvertex[(7 - i_direction) % 4][1]; + + double c0, c1, w_2; + c0 = center[0]; + c1 = center[1]; + w_2 = i_width / 2.0; + + o_ppos3d[0][0] = c0 - w_2;// center[0] - w/2.0; + o_ppos3d[0][1] = c1 + w_2;// center[1] + w/2.0; + o_ppos3d[1][0] = c0 + w_2;// center[0] + w/2.0; + o_ppos3d[1][1] = c1 + w_2;// center[1] + w/2.0; + o_ppos3d[2][0] = c0 + w_2;// center[0] + w/2.0; + o_ppos3d[2][1] = c1 - w_2;// center[1] - w/2.0; + o_ppos3d[3][0] = c0 - w_2;// center[0] - w/2.0; + o_ppos3d[3][1] = c1 - w_2;// center[1] - w/2.0; + return; + } + + private final double[][] wk_transMat_pos3d = new double[P_MAX][3];// pos3d[P_MAX][3]; + private final double[][] wk_transMat_ppos2d = new double[4][2]; + private final double[][] wk_transMat_ppos3d = new double[4][2]; + private final double[] wk_transMat_off = new double[3]; + private final double[][] wk_transMat_pos2d = new double[P_MAX][2];// pos2d[P_MAX][2]; + private final NyARMat wk_transMat_mat_b = new NyARMat(3, + NUMBER_OF_VERTEX * 2); + private final NyARMat wk_transMat_mat_d = new NyARMat(3, 3); + private final double[] wk_transMat_mat_trans = new double[3]; + + /** + * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double + * width, double conv[3][4] ) 演算シーケンス最適化のため、arGetTransMat3等の関数フラグメントを含みます。 + * 保持している変換行列を更新する。 + * + * @param square + * 計算対象のNyARSquareオブジェクト + * @param i_direction + * @param width + * @return + * @throws NyARException + */ + public double transMat(NyARSquare square, int i_direction, double width, + NyARTransMatResult o_result_conv) throws NyARException { + double[][] ppos2d = wk_transMat_ppos2d; + double[][] ppos3d = wk_transMat_ppos3d; + double[] off = wk_transMat_off; + double[][] pos3d = wk_transMat_pos3d; + + // rotationの初期化 + transrot.initRot(square, i_direction); + + // ppos2dとppos3dの初期化 + init_transMat_ppos(square, i_direction, width, ppos2d, ppos3d); + + // arGetTransMat3の前段処理(pos3dとoffを初期化) + double[][] pos2d = this.wk_transMat_pos2d; + final NyARMat mat_b = this.wk_transMat_mat_b; + final NyARMat mat_d = this.wk_transMat_mat_d; + + arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, + mat_d); + + double err = -1; + double[] trans = this.wk_transMat_mat_trans; + for (int i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { + // + err = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans); + // // + if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { + break; + } + } + // マトリクスの保存 + o_result_conv.updateMatrixValue(this.transrot, off, trans); + return err; + } + + private final NyARTransMatResult wk_transMatContinue_result = new NyARTransMatResult(); + + /** + * double arGetTransMatCont( ARMarkerInfo *marker_info, double + * prev_conv[3][4],double center[2], double width, double conv[3][4] ) + * + * @param i_square + * @param i_direction + * マーカーの方位を指定する。 + * @param i_width + * @param io_result_conv + * 計算履歴を持つNyARTransMatResultオブジェクトを指定する。 + * 履歴を持たない場合は、transMatと同じ処理を行う。 + * @return + * @throws NyARException + */ + public double transMatContinue(NyARSquare i_square, int i_direction, + double i_width, NyARTransMatResult io_result_conv) + throws NyARException { + // io_result_convが初期値なら、transMatで計算する。 + if (!io_result_conv.hasValue()) { + return this + .transMat(i_square, i_direction, i_width, io_result_conv); + } + + double[][] ppos2d = wk_transMat_ppos2d; + double[][] ppos3d = wk_transMat_ppos3d; + double[] off = wk_transMat_off; + double[][] pos3d = wk_transMat_pos3d; + + // arGetTransMatContSub計算部分 + transrot.initRotByPrevResult(io_result_conv); + + // ppos2dとppos3dの初期化 + init_transMat_ppos(i_square, i_direction, i_width, ppos2d, ppos3d); + + // arGetTransMat3の前段処理(pos3dとoffを初期化) + double[][] pos2d = this.wk_transMat_pos2d; + final NyARMat mat_b = this.wk_transMat_mat_b; + final NyARMat mat_d = this.wk_transMat_mat_d; + + // transMatに必要な初期値を計算 + arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, + mat_d); + + double err1, err2; + int i; + + err1 = err2 = -1; + double[] trans = this.wk_transMat_mat_trans; + for (i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { + err1 = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans); + if (err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { + // 十分な精度を達成できたらブレーク + break; + } + } + // 値を保存 + io_result_conv.updateMatrixValue(this.transrot, off, trans); + + // エラー値が許容範囲でなければTransMatをやり直し + if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) { + NyARTransMatResult result2 = this.wk_transMatContinue_result; + // transMatを実行(初期化値は共用) + transrot.initRot(i_square, i_direction); + err2 = transMat(i_square, i_direction, i_width, result2); + // transmMatここまで + if (err2 < err1) { + // 良い値が取れたら、差換え + io_result_conv.copyFrom(result2); + err1 = err2; + } + } + return err1; + } + + private final NyARMat wk_arGetTransMat3_mat_a = new NyARMat( + NUMBER_OF_VERTEX * 2, 3); + + /** + * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。 + * 計算結果から再帰的に変更される可能性が無いので、切り離し。 + * + * @param i_ppos3d + * 入力配列[num][3] + * @param o_pos3d + * 出力配列[P_MAX][3] + * @param o_off + * [3] + * @throws NyARException + */ + private final void arGetTransMat3_initTransMat(double[][] i_ppos3d, + double[][] i_ppos2d, double[][] o_pos2d, double[][] o_pos3d, + double[] o_off, NyARMat o_mat_b, NyARMat o_mat_d) + throws NyARException { + double pmax0, pmax1, pmax2, pmin0, pmin1, pmin2; + int i; + pmax0 = pmax1 = pmax2 = -10000000000.0; + pmin0 = pmin1 = pmin2 = 10000000000.0; + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + if (i_ppos3d[i][0] > pmax0) { + pmax0 = i_ppos3d[i][0]; + } + if (i_ppos3d[i][0] < pmin0) { + pmin0 = i_ppos3d[i][0]; + } + if (i_ppos3d[i][1] > pmax1) { + pmax1 = i_ppos3d[i][1]; + } + if (i_ppos3d[i][1] < pmin1) { + pmin1 = i_ppos3d[i][1]; + } + /* + * オリジナルでもコメントアウト if( ppos3d[i][2] > pmax[2] ) pmax[2] = + * ppos3d[i][2]; if( ppos3d[i][2] < pmin[2] ) pmin[2] = + * ppos3d[i][2]; + */ + } + o_off[0] = -(pmax0 + pmin0) / 2.0; + o_off[1] = -(pmax1 + pmin1) / 2.0; + o_off[2] = -(pmax2 + pmin2) / 2.0; + + double[] o_pos3d_pt; + double[] i_pos_pd_pt; + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + o_pos3d_pt = o_pos3d[i]; + i_pos_pd_pt = i_ppos3d[i]; + o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0]; + o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1]; + o_pos3d_pt[2] = 0.0; + } + // ココから先でarGetTransMatSubの初期化処理 + // arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。 + + if (arFittingMode == AR_FITTING_TO_INPUT) { + // arParamIdeal2Observをバッチ処理 + param.ideal2ObservBatch(i_ppos2d, o_pos2d, NUMBER_OF_VERTEX); + } else { + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + o_pos2d[i][0] = i_ppos2d[i][0]; + o_pos2d[i][1] = i_ppos2d[i][1]; + } + } + + // 変換マトリクスdとbの準備(arGetTransMatSubの一部) + final double cpara[] = param.get34Array(); + final NyARMat mat_a = this.wk_arGetTransMat3_mat_a; + final double[][] a_array = mat_a.getArray(); + + // mat_bの設定 + final double[][] b_array = o_mat_b.getArray(); + + int x2; + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + x2 = i * 2; + // + a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0] + // = + // mat_b->m[num*0+j*2] + // = + // cpara[0][0]; + a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1] + // = + // mat_b->m[num*2+j*2] + // = + // cpara[0][1]; + a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - o_pos2d[i][0];// mat_a->m[j*6+2] + // = + // mat_b->m[num*4+j*2] + // = + // cpara[0][2] + // - + // pos2d[j][0]; + a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] = + // mat_b->m[num*0+j*2+1] + // = 0.0; + a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4] + // = + // mat_b->m[num*2+j*2+1] + // = + // cpara[1][1]; + a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] + - o_pos2d[i][1];// mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = + // cpara[1][2] - pos2d[j][1]; + } + + // mat_d + o_mat_d.matrixMul(o_mat_b, mat_a); + o_mat_d.matrixSelfInv(); + } + + private final NyARMat wk_arGetTransMatSub_mat_c = new NyARMat( + NUMBER_OF_VERTEX * 2, 1); + private final NyARMat wk_arGetTransMatSub_mat_e = new NyARMat(3, 1); + private final NyARMat wk_arGetTransMatSub_mat_f = new NyARMat(3, 1); + + /** + * static double arGetTransMatSub( double rot[3][3], double + * ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double + * *dist_factor, double cpara[3][4] ) Optimize:2008.04.20:STEP[1033→1004] + * + * @param i_ppos2d + * @param i_pos3d + * @param i_mat_b + * 演算用行列b + * @param i_mat_d + * 演算用行列d + * @return + * @throws NyARException + */ + private final double arGetTransMatSub(double i_ppos2d[][], + double i_pos3d[][], NyARMat i_mat_b, NyARMat i_mat_d, + double[] o_trans) throws NyARException { + double cpara[] = param.get34Array(); + NyARMat mat_c, mat_e, mat_f;// ARMat *mat_a, *mat_b, *mat_c, *mat_d, + // *mat_e, *mat_f; + + double wx, wy, wz; + double ret; + int i; + + mat_c = this.wk_arGetTransMatSub_mat_c;// 次処理で値をもらうので、初期化の必要は無い。 + double[][] c_array = mat_c.getArray(); + double[] rot = transrot.getArray(); + double[] i_pos3d_pt; + int x2; + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + x2 = i * 2; + i_pos3d_pt = i_pos3d[i]; + wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] + * i_pos3d_pt[2]; + wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] + * i_pos3d_pt[2]; + wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] + * i_pos3d_pt[2]; + c_array[x2][0] = wz * i_ppos2d[i][0] - cpara[0 * 4 + 0] * wx + - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] + // = wz * + // pos2d[j][0]- + // cpara[0][0]*wx + // - + // cpara[0][1]*wy + // - + // cpara[0][2]*wz; + c_array[x2 + 1][0] = wz * i_ppos2d[i][1] - cpara[1 * 4 + 1] * wy + - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz * + // pos2d[j][1]- cpara[1][1]*wy - + // cpara[1][2]*wz; + } + mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。 + mat_f = this.wk_arGetTransMatSub_mat_f;// 次処理で値をもらうので、初期化の必要は無い。 + double[][] f_array = mat_f.getArray(); + + mat_e.matrixMul(i_mat_b, mat_c); + mat_f.matrixMul(i_mat_d, mat_e); + + // double[] trans=wk_arGetTransMatSub_trans;//double trans[3]; + o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; + o_trans[1] = f_array[1][0]; + o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; + ret = transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d); + for (i = 0; i < NUMBER_OF_VERTEX; i++) { + x2 = i * 2; + i_pos3d_pt = i_pos3d[i]; + wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] + * i_pos3d_pt[2]; + wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] + * i_pos3d_pt[2]; + wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] + * i_pos3d_pt[2]; + c_array[x2][0] = wz * i_ppos2d[i][0] - cpara[0 * 4 + 0] * wx + - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] + // = wz * + // pos2d[j][0]- + // cpara[0][0]*wx + // - + // cpara[0][1]*wy + // - + // cpara[0][2]*wz; + c_array[x2 + 1][0] = wz * i_ppos2d[i][1] - cpara[1 * 4 + 1] * wy + - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] = wz * + // pos2d[j][1]- cpara[1][1]*wy - + // cpara[1][2]*wz; + } + + mat_e.matrixMul(i_mat_b, mat_c); + mat_f.matrixMul(i_mat_d, mat_e); + o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; + o_trans[1] = f_array[1][0]; + o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; + ret = transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d); + return ret; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARTransRot.java b/src/jp/nyatla/nyartoolkit/core/NyARTransRot.java new file mode 100644 index 0000000..40cda6b --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARTransRot.java @@ -0,0 +1,1258 @@ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + +interface NyARTransRot +{ + public double[] getArray(); + /** + * + * @param trans + * @param vertex + * @param pos2d + * [n*2]配列 + * @return + * @throws NyARException + */ + public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException; + public void initRot(NyARSquare marker_info,int i_direction) throws NyARException; + public void initRotByPrevResult(NyARTransMatResult i_prev_result); +} + +/** + * NyARTransRot派生クラスで共通に使いそうな関数類をまとめたもの。 + * + */ +abstract class NyARTransRot_OptimizeCommon implements NyARTransRot +{ + protected final int number_of_vertex; + protected final double[] array=new double[9]; + protected final NyARParam cparam; + public final void initRotByPrevResult(NyARTransMatResult i_prev_result) + { + double[][] prev_array=i_prev_result.getArray(); + double[] pt; + final double[] L_rot=this.array; + pt=prev_array[0]; + L_rot[0*3+0]=pt[0]; + L_rot[0*3+1]=pt[1]; + L_rot[0*3+2]=pt[2]; + pt=prev_array[1]; + L_rot[1*3+0]=pt[0]; + L_rot[1*3+1]=pt[1]; + L_rot[1*3+2]=pt[2]; + pt=prev_array[2]; + L_rot[2*3+0]=pt[0]; + L_rot[2*3+1]=pt[1]; + L_rot[2*3+2]=pt[2]; + } + public final double[] getArray() + { + return this.array; + } + /** + * インスタンスを準備します。 + * @param i_param + * nullを指定した場合、一部の関数が使用不能になります。 + */ + public NyARTransRot_OptimizeCommon(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + number_of_vertex=i_number_of_vertex; + cparam=i_param; + } + + private final double[] wk_check_dir_world=new double[6]; + private final double[] wk_check_dir_camera=new double[4]; + private final NyARMat wk_check_dir_NyARMat=new NyARMat( 3, 3 ); + /** + * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) + * Optimize:STEP[526->468] + * @param dir + * @param st + * @param ed + * @param cpara + * + * @throws NyARException + */ + protected final void check_dir( double dir[], double st[], double ed[],double cpara[]) throws NyARException + { + double h; + int i, j; + + NyARMat mat_a = this.wk_check_dir_NyARMat;//ここ、事前に初期化できそう + double[][] a_array=mat_a.getArray(); + for(j=0;j<3;j++){ + for(i=0;i<3;i++){ + a_array[j][i]=cpara[j*4+i];//m[j*3+i] = cpara[j][i]; + } + + } + // JartkException.trap("未チェックのパス"); + mat_a.matrixSelfInv(); + double[] world=wk_check_dir_world;//[2][3]; + // + //world[0][0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ mat_a->m[2]*10.0; + //world[0][1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ mat_a->m[5]*10.0; + //world[0][2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ mat_a->m[8]*10.0; + //world[1][0] = world[0][0] + dir[0]; + //world[1][1] = world[0][1] + dir[1]; + //world[1][2] = world[0][2] + dir[2]; + world[0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ mat_a->m[2]*10.0; + world[1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ mat_a->m[5]*10.0; + world[2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ mat_a->m[8]*10.0; + world[3] = world[0] + dir[0]; + world[4] = world[1] + dir[1]; + world[5] = world[2] + dir[2]; + // + + double[] camera=wk_check_dir_camera;//[2][2]; + for( i = 0; i < 2; i++ ) { + h = cpara[2*4+0] * world[i*3+0]+ cpara[2*4+1] * world[i*3+1]+ cpara[2*4+2] * world[i*3+2]; + if( h == 0.0 ){ + throw new NyARException(); + } + camera[i*2+0] = (cpara[0*4+0] * world[i*3+0]+ cpara[0*4+1] * world[i*3+1]+ cpara[0*4+2] * world[i*3+2]) / h; + camera[i*2+1] = (cpara[1*4+0] * world[i*3+0]+ cpara[1*4+1] * world[i*3+1]+ cpara[1*4+2] * world[i*3+2]) / h; + } + // + //v[0][0] = ed[0] - st[0]; + //v[0][1] = ed[1] - st[1]; + //v[1][0] = camera[1][0] - camera[0][0]; + //v[1][1] = camera[1][1] - camera[0][1]; + double v=(ed[0]-st[0])*(camera[2]-camera[0])+(ed[1]-st[1])*(camera[3]-camera[1]); + // + if(v<0) {//if( v[0][0]*v[1][0] + v[0][1]*v[1][1] < 0 ) { + dir[0] = -dir[0]; + dir[1] = -dir[1]; + dir[2] = -dir[2]; + } + } + /*int check_rotation( double rot[2][3] )*/ + protected final static void check_rotation( double rot[][] ) throws NyARException + { + double[] v1=new double[3], v2=new double[3], v3=new double[3]; + double ca, cb, k1, k2, k3, k4; + double a, b, c, d; + double p1, q1, r1; + double p2, q2, r2; + double p3, q3, r3; + double p4, q4, r4; + double w; + double e1, e2, e3, e4; + int f; + + v1[0] = rot[0][0]; + v1[1] = rot[0][1]; + v1[2] = rot[0][2]; + v2[0] = rot[1][0]; + v2[1] = rot[1][1]; + v2[2] = rot[1][2]; + v3[0] = v1[1]*v2[2] - v1[2]*v2[1]; + v3[1] = v1[2]*v2[0] - v1[0]*v2[2]; + v3[2] = v1[0]*v2[1] - v1[1]*v2[0]; + w = Math.sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] ); + if( w == 0.0 ){ + throw new NyARException(); + } + v3[0] /= w; + v3[1] /= w; + v3[2] /= w; + + cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; + if( cb < 0 ) cb *= -1.0; + ca = (Math.sqrt(cb+1.0) + Math.sqrt(1.0-cb)) * 0.5; + + if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) { + f = 0; + } + else { + if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) { + w = v1[1]; v1[1] = v1[2]; v1[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 1; + } + else { + w = v1[0]; v1[0] = v1[2]; v1[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 2; + } + } + if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ){ + throw new NyARException(); + } + k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]); + k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]); + k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]); + k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]); + + a = k1*k1 + k3*k3 + 1; + b = k1*k2 + k3*k4; + c = k2*k2 + k4*k4 - 1; + + d = b*b - a*c; + if( d < 0 ){ + throw new NyARException(); + } + r1 = (-b + Math.sqrt(d))/a; + p1 = k1*r1 + k2; + q1 = k3*r1 + k4; + r2 = (-b - Math.sqrt(d))/a; + p2 = k1*r2 + k2; + q2 = k3*r2 + k4; + if( f == 1 ) { + w = q1; q1 = r1; r1 = w; + w = q2; q2 = r2; r2 = w; + w = v1[1]; v1[1] = v1[2]; v1[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 0; + } + if( f == 2 ) { + w = p1; p1 = r1; r1 = w; + w = p2; p2 = r2; r2 = w; + w = v1[0]; v1[0] = v1[2]; v1[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 0; + } + + if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) { + f = 0; + }else { + if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) { + w = v2[1]; v2[1] = v2[2]; v2[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 1; + } + else { + w = v2[0]; v2[0] = v2[2]; v2[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 2; + } + } + if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ){ + throw new NyARException(); + } + k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]); + k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]); + k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]); + k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]); + + a = k1*k1 + k3*k3 + 1; + b = k1*k2 + k3*k4; + c = k2*k2 + k4*k4 - 1; + + d = b*b - a*c; + if( d < 0 ){ + throw new NyARException(); + } + r3 = (-b + Math.sqrt(d))/a; + p3 = k1*r3 + k2; + q3 = k3*r3 + k4; + r4 = (-b - Math.sqrt(d))/a; + p4 = k1*r4 + k2; + q4 = k3*r4 + k4; + if( f == 1 ) { + w = q3; q3 = r3; r3 = w; + w = q4; q4 = r4; r4 = w; + w = v2[1]; v2[1] = v2[2]; v2[2] = w; + w = v3[1]; v3[1] = v3[2]; v3[2] = w; + f = 0; + } + if( f == 2 ) { + w = p3; p3 = r3; r3 = w; + w = p4; p4 = r4; r4 = w; + w = v2[0]; v2[0] = v2[2]; v2[2] = w; + w = v3[0]; v3[0] = v3[2]; v3[2] = w; + f = 0; + } + + e1 = p1*p3+q1*q3+r1*r3; + if( e1 < 0 ){ + e1 = -e1; + } + e2 = p1*p4+q1*q4+r1*r4; + if( e2 < 0 ){ + e2 = -e2; + } + e3 = p2*p3+q2*q3+r2*r3; + if( e3 < 0 ){ + e3 = -e3; + } + e4 = p2*p4+q2*q4+r2*r4; + if( e4 < 0 ){ + e4 = -e4; + } + if( e1 < e2 ) { + if( e1 < e3 ) { + if( e1 < e4 ) { + rot[0][0] = p1; + rot[0][1] = q1; + rot[0][2] = r1; + rot[1][0] = p3; + rot[1][1] = q3; + rot[1][2] = r3; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + else { + if( e3 < e4 ) { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p3; + rot[1][1] = q3; + rot[1][2] = r3; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + } + else { + if( e2 < e3 ) { + if( e2 < e4 ) { + rot[0][0] = p1; + rot[0][1] = q1; + rot[0][2] = r1; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + else { + if( e3 < e4 ) { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p3; + rot[1][1] = q3; + rot[1][2] = r3; + } + else { + rot[0][0] = p2; + rot[0][1] = q2; + rot[0][2] = r2; + rot[1][0] = p4; + rot[1][1] = q4; + rot[1][2] = r4; + } + } + } + } + /** + * パラメタa,b,cからrotを計算してインスタンスに保存する。 + * rotを1次元配列に変更 + * Optimize:2008.04.20:STEP[253→186] + * @param a + * @param b + * @param c + * @param o_rot + */ + protected final static void arGetRot( double a, double b, double c,double[] o_rot) + { + double sina, sinb, sinc; + double cosa, cosb, cosc; + + sina = Math.sin(a); + cosa = Math.cos(a); + sinb = Math.sin(b); + cosb = Math.cos(b); + sinc = Math.sin(c); + cosc = Math.cos(c); + //Optimize + double CACA,SASA,SACA,SASB,CASB,SACACB; + CACA =cosa*cosa; + SASA =sina*sina; + SACA =sina*cosa; + SASB =sina*sinb; + CASB =cosa*sinb; + SACACB=SACA*cosb; + + + o_rot[0] = CACA*cosb*cosc+SASA*cosc+SACACB*sinc-SACA*sinc; + o_rot[1] = -CACA*cosb*sinc-SASA*sinc+SACACB*cosc-SACA*cosc; + o_rot[2] = CASB; + o_rot[3] = SACACB*cosc-SACA*cosc+SASA*cosb*sinc+CACA*sinc; + o_rot[4] = -SACACB*sinc+SACA*sinc+SASA*cosb*cosc+CACA*cosc; + o_rot[5] = SASB; + o_rot[6] = -CASB*cosc-SASB*sinc; + o_rot[7] = CASB*sinc-SASB*cosc; + o_rot[8] = cosb; + } + /** + * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) + * Optimize:2008.04.20:STEP[481→433] + * @param rot + * 2次元配列を1次元化してあります。 + * @param o_abc + * @return + */ + protected final int arGetAngle(double[] o_abc) + { + double a, b, c,tmp; + double sina, cosa, sinb, cosb, sinc, cosc; + double[] rot=array; + if( rot[8] > 1.0 ) {//if( rot[2][2] > 1.0 ) { + rot[8] = 1.0;//rot[2][2] = 1.0; + }else if( rot[8] < -1.0 ) {//}else if( rot[2][2] < -1.0 ) { + rot[8] = -1.0;//rot[2][2] = -1.0; + } + cosb = rot[8];//cosb = rot[2][2]; + b = Math.acos( cosb ); + sinb = Math.sin( b ); + if( b >= 0.000001 || b <= -0.000001) { + cosa = rot[2] / sinb;//cosa = rot[0][2] / sinb; + sina = rot[5] / sinb;//sina = rot[1][2] / sinb; + if( cosa > 1.0 ) { + /* printf("cos(alph) = %f\n", cosa); */ + cosa = 1.0; + sina = 0.0; + } + if( cosa < -1.0 ) { + /* printf("cos(alph) = %f\n", cosa); */ + cosa = -1.0; + sina = 0.0; + } + if( sina > 1.0 ) { + /* printf("sin(alph) = %f\n", sina); */ + sina = 1.0; + cosa = 0.0; + } + if( sina < -1.0 ) { + /* printf("sin(alph) = %f\n", sina); */ + sina = -1.0; + cosa = 0.0; + } + a = Math.acos( cosa ); + if( sina < 0 ){ + a = -a; + } + // + //sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]); + //cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]); + tmp = (rot[2]*rot[2]+rot[5]*rot[5]); + sinc = (rot[7]*rot[2]-rot[6]*rot[5])/ tmp; + cosc = -(rot[2]*rot[6]+rot[5]*rot[7])/ tmp; + // + + if( cosc > 1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = 1.0; + sinc = 0.0; + } + if( cosc < -1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = -1.0; + sinc = 0.0; + } + if( sinc > 1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = 1.0; + cosc = 0.0; + } + if( sinc < -1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = -1.0; + cosc = 0.0; + } + c = Math.acos( cosc ); + if( sinc < 0 ){ + c = -c; + } + }else { + a = b = 0.0; + cosa = cosb = 1.0; + sina = sinb = 0.0; + cosc = rot[0];//cosc = rot[0][0]; + sinc = rot[1];//sinc = rot[1][0]; + if( cosc > 1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = 1.0; + sinc = 0.0; + } + if( cosc < -1.0 ) { + /* printf("cos(r) = %f\n", cosc); */ + cosc = -1.0; + sinc = 0.0; + } + if( sinc > 1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = 1.0; + cosc = 0.0; + } + if( sinc < -1.0 ) { + /* printf("sin(r) = %f\n", sinc); */ + sinc = -1.0; + cosc = 0.0; + } + c = Math.acos( cosc ); + if( sinc < 0 ){ + c = -c; + } + } + o_abc[0]=a;//wa.value=a;//*wa = a; + o_abc[1]=b;//wb.value=b;//*wb = b; + o_abc[2]=c;//wc.value=c;//*wc = c; + return 0; + } +} + +/** + * NyARModifyMatrixの最適化バージョン1 + * 配列の１次元化、計算ステップの圧縮等の最適化をしてみた。 + * + */ +class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon +{ + public NyARTransRot_O1(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + super(i_param,i_number_of_vertex); + } + /** + * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] ) + * Optimize:2008.04.20:STEP[716→698] + * @param marker_info + * @param i_direction + * @param i_param + * @throws NyARException + */ + public final void initRot(NyARSquare marker_info,int i_direction) throws NyARException + { + double cpara[]= cparam.get34Array(); + double[][] wdir=new double[3][3]; + double w, w1, w2, w3; + int dir; + int j; + + dir = i_direction; + + for( j = 0; j < 2; j++ ) { + w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1]; + w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2]; + w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0]; + + wdir[j][0] = w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+ w2*cpara[1*4+1]- w3*cpara[0*4+1]; + wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+ w3*cpara[0*4+0]; + wdir[j][2] = w1*cpara[0*4+0]*cpara[1*4+1]; + w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] ); + wdir[j][0] /= w; + wdir[j][1] /= w; + wdir[j][2] /= w; + } + + //以下3ケースは、計算エラーのときは例外が発生する。 + check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara); + + check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara); + + check_rotation(wdir); + + + wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1]; + wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2]; + wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0]; + w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] ); + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + /* + if( wdir[2][2] < 0 ) { + wdir[2][0] /= -w; + wdir[2][1] /= -w; + wdir[2][2] /= -w; + } + else { + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + } + */ + // + //rot[0][0] = wdir[0][0]; + //rot[1][0] = wdir[0][1]; + //rot[2][0] = wdir[0][2]; + //rot[0][1] = wdir[1][0]; + //rot[1][1] = wdir[1][1]; + //rot[2][1] = wdir[1][2]; + //rot[0][2] = wdir[2][0]; + //rot[1][2] = wdir[2][1]; + //rot[2][2] = wdir[2][2]; + double[] rot=this.array; + rot[0] = wdir[0][0]; + rot[3] = wdir[0][1]; + rot[6] = wdir[0][2]; + rot[1] = wdir[1][0]; + rot[4] = wdir[1][1]; + rot[7] = wdir[1][2]; + rot[2] = wdir[2][0]; + rot[5] = wdir[2][1]; + rot[8] = wdir[2][2]; + // + } + private final double[] wk_arModifyMatrix_combo=new double[12];//[3][4]; + private final double[] wk_arModifyMatrix_abc=new double[3]; + private final double[] wk_arModifyMatrix_rot=new double[9]; + /** + * Optimize:2008.04.20:STEP[456→-] + * @param rot + * [3x3]配列 + * @param trans + * @param vertex + * @param pos2d + * @param num + * @return + */ + public final double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException + { + int num=this.number_of_vertex; + double factor; + double a1, b1, c1; + double a2, b2, c2; + double ma = 0.0, mb = 0.0, mc = 0.0; + double hx, hy, h, x, y; + double err, minerr=0; + int t1, t2, t3; + int s1 = 0, s2 = 0, s3 = 0; + int i, j; + double[] combo=this.wk_arModifyMatrix_combo;//arGetNewMatrixで初期化されるので初期化不要//new double[3][4]; + double[] abc=wk_arModifyMatrix_abc; + double[] rot=wk_arModifyMatrix_rot; + + arGetAngle(abc);//arGetAngle( rot, &a, &b, &c ); + a2 = abc[0]; + b2 = abc[1]; + c2 = abc[2]; + factor = 10.0*Math.PI/180.0; + for( j = 0; j < 10; j++ ) { + minerr = 1000000000.0; + for(t1=-1;t1<=1;t1++) { + for(t2=-1;t2<=1;t2++) { + for(t3=-1;t3<=1;t3++) { + a1 = a2 + factor*t1; + b1 = b2 + factor*t2; + c1 = c2 + factor*t3; + arGetRot( a1, b1, c1,rot); + arGetNewMatrix(rot,trans, null, combo ); + err = 0.0; + for( i = 0; i < num; i++ ) { + hx = combo[0] * vertex[i][0]+ combo[1] * vertex[i][1]+ combo[2] * vertex[i][2]+ combo[3]; + hy = combo[4] * vertex[i][0]+ combo[5] * vertex[i][1]+ combo[6] * vertex[i][2]+ combo[7]; + h = combo[8] * vertex[i][0]+ combo[9] * vertex[i][1]+ combo[10] * vertex[i][2]+ combo[11]; + x = hx / h; + y = hy / h; + err += (pos2d[i][0] - x) * (pos2d[i][0] - x)+ (pos2d[i][1] - y) * (pos2d[i][1] - y); + } + if( err < minerr ) { + minerr = err; + ma = a1; + mb = b1; + mc = c1; + s1 = t1; + s2 = t2; + s3 = t3; + } + } + } + } + if( s1 == 0 && s2 == 0 && s3 == 0 ){ + factor *= 0.5; + } + a2 = ma; + b2 = mb; + c2 = mc; + } + arGetRot(ma, mb, mc,this.array); + /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ + return minerr/num; + } + private final double[] wk_cpara2_arGetNewMatrix=new double[12];//[3][4]; + /** + * Optimize:2008.04.20:STEP[569->432] + * @param i_rot + * [9] + * @param trans + * @param trans2 + * @param ret + * double[3x4]配列 + * @return + */ + private final int arGetNewMatrix(double[] i_rot,double trans[], double trans2[][], double ret[]) throws NyARException + { + final double cpara[]=cparam.get34Array(); + final double[] cpara2; //この関数で初期化される。 + int j,j_idx; +// double[] cpara_pt; + //cparaの2次元配列→1次元に変換して計算 + if( trans2 != null ) { + cpara2=wk_cpara2_arGetNewMatrix; //この関数で初期化される。 + + for( j = 0; j < 3; j++ ) { +// Optimize(使わないから最適化してない) + NyARException.trap("未チェックのパス"); + cpara2[j*4+0] = cpara[j*4+0] * trans2[0][0]+ cpara[j*4+1] * trans2[1][0]+ cpara[j*4+2] * trans2[2][0]; + cpara2[j*4+1] = cpara[j*4+0] * trans2[0][1]+ cpara[j*4+1] * trans2[1][1]+ cpara[j*4+2] * trans2[2][1]; + cpara2[j*4+2] = cpara[j*4+0] * trans2[0][2]+ cpara[j*4+1] * trans2[1][2]+ cpara[j*4+2] * trans2[2][2]; + cpara2[j*4+3] = cpara[j*4+0] * trans2[0][3]+ cpara[j*4+1] * trans2[1][3]+ cpara[j*4+2] * trans2[2][3]; + } + }else{ + cpara2=cpara;//cparaの値をそのまま使う + } + for( j = 0; j < 3; j++ ) { + //cpara2_pt=cpara2[j]; + j_idx=j*4; + // + //ret[j][0] = cpara2_pt[0] * rot[0][0]+ cpara2_pt[1] * rot[1][0]+ cpara2_pt[2] * rot[2][0]; + //ret[j][1] = cpara2_pt[0] * rot[0][1]+ cpara2_pt[1] * rot[1][1]+ cpara2_pt[2] * rot[2][1]; + //ret[j][2] = cpara2_pt[0] * rot[0][2]+ cpara2_pt[1] * rot[1][2]+ cpara2_pt[2] * rot[2][2]; + //ret[j][3] = cpara2_pt[0] * trans[0]+ cpara2_pt[1] * trans[1]+ cpara2_pt[2] * trans[2]+ cpara2_pt[3]; + ret[j_idx+0] = cpara2[j_idx+0] * i_rot[0]+ cpara2[j_idx+1] * i_rot[3]+ cpara2[j_idx+2] * i_rot[6]; + ret[j_idx+1] = cpara2[j_idx+0] * i_rot[1]+ cpara2[j_idx+1] * i_rot[4]+ cpara2[j_idx+2] * i_rot[7]; + ret[j_idx+2] = cpara2[j_idx+0] * i_rot[2]+ cpara2[j_idx+1] * i_rot[5]+ cpara2[j_idx+2] * i_rot[8]; + ret[j_idx+3] = cpara2[j_idx+0] * trans[0]+ cpara2[j_idx+1] * trans[1]+ cpara2[j_idx+2] * trans[2]+ cpara2[j_idx+3]; + // + } + return(0); + } +} + +/** + * NyARModifyMatrixの最適化バージョン2 + * 計算手順の変更、構造変更など含む最適化をしたもの + * + */ +class NyARTransRot_O2 extends NyARTransRot_OptimizeCommon +{ + public NyARTransRot_O2(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + super(i_param,i_number_of_vertex); + } + + //private double CACA,SASA,SACA,CA,SA; + private double CACA,SASA,SACA,CA,SA; + final public void arGetRotA( double a) + { + double sina,cosa; + sina = Math.sin(a); + cosa = Math.cos(a); + //Optimize + CACA=cosa*cosa; + SASA=sina*sina; + SACA=sina*cosa; + CA=cosa; + SA=sina; + } + private double CACACB,SACACB,SASACB,CASB,SASB; + final public void arGetRotB(double b,double[] o_rot) + { + double sinb,cosb; + sinb = Math.sin(b); + cosb = Math.cos(b); + CACACB=CACA*cosb; + SACACB=SACA*cosb; + SASACB=SASA*cosb; + CASB=CA*sinb; + SASB=SA*sinb; + o_rot[2] = CASB; + o_rot[5] = SASB; + o_rot[8] = cosb; + } + /** + * 分割arGetRot + * @param c + */ + public final void arGetRotC(double c,double[] o_rot) + { + double sinc,cosc; + sinc = Math.sin(c); + cosc = Math.cos(c); + double SACASC,SACACBSC,SACACBCC,SACACC; + SACASC=SACA*sinc; + SACACC=SACA*cosc; + SACACBSC=SACACB*sinc; + SACACBCC=SACACB*cosc; + o_rot[0] = CACACB*cosc+SASA*cosc+SACACBSC-SACASC; + o_rot[1] = -CACACB*sinc-SASA*sinc+SACACBCC-SACACC; + o_rot[3] = SACACBCC-SACACC+SASACB*sinc+CACA*sinc; + o_rot[4] = -SACACBSC+SACASC+SASACB*cosc+CACA*cosc; + o_rot[6] = -CASB*cosc-SASB*sinc; + o_rot[7] = CASB*sinc-SASB*cosc; + } + private final double[][] wk_initRot_wdir=new double[3][3]; + /** + * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] ) + * Optimize:2008.04.20:STEP[716→698] + * @param marker_info + * @param i_direction + * @param i_param + * @throws NyARException + */ + public void initRot(NyARSquare marker_info,int i_direction) throws NyARException + { + double cpara[]= cparam.get34Array(); + double[][] wdir=wk_initRot_wdir;//この関数で初期化される + double w, w1, w2, w3; + int dir; + int j; + + dir = i_direction; + + for( j = 0; j < 2; j++ ) { + w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1]; + w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2]; + w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0]; + + wdir[j][0] = w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+ w2*cpara[1*4+1]- w3*cpara[0*4+1]; + wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+ w3*cpara[0*4+0]; + wdir[j][2] = w1*cpara[0*4+0]*cpara[1*4+1]; + w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] ); + wdir[j][0] /= w; + wdir[j][1] /= w; + wdir[j][2] /= w; + } + + //以下3ケースは、計算エラーのときは例外が発生する。 + check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara); + + check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara); + + check_rotation(wdir); + + + wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1]; + wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2]; + wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0]; + w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] ); + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + // + //rot[0][0] = wdir[0][0]; + //rot[1][0] = wdir[0][1]; + //rot[2][0] = wdir[0][2]; + //rot[0][1] = wdir[1][0]; + //rot[1][1] = wdir[1][1]; + //rot[2][1] = wdir[1][2]; + //rot[0][2] = wdir[2][0]; + //rot[1][2] = wdir[2][1]; + //rot[2][2] = wdir[2][2]; + double[] rot=this.array; + rot[0] = wdir[0][0]; + rot[3] = wdir[0][1]; + rot[6] = wdir[0][2]; + rot[1] = wdir[1][0]; + rot[4] = wdir[1][1]; + rot[7] = wdir[1][2]; + rot[2] = wdir[2][0]; + rot[5] = wdir[2][1]; + rot[8] = wdir[2][2]; + // + } + private final double[] wk_arModifyMatrix_combo=new double[12];//[3][4]; + private final double[] wk_arModifyMatrix_abc=new double[3]; + private final double[] wk_arModifyMatrix_rot=new double[9]; + /** + * arGetRot計算を階層化したModifyMatrix + * @param nyrot + * @param trans + * @param vertex + * @param pos2d + * @param num + * @return + * @throws NyARException + */ + public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException + { + int num=this.number_of_vertex; + double factor; + double a1, b1, c1; + double a2, b2, c2; + double ma = 0.0, mb = 0.0, mc = 0.0; + double hx, hy, h, x, y; + double err, minerr=0; + int t1, t2, t3; + int s1 = 0, s2 = 0, s3 = 0; + int i, j; + final double[] combo=this.wk_arModifyMatrix_combo;//arGetNewMatrixで初期化されるので初期化不要//new double[3][4]; + final double[] abc=wk_arModifyMatrix_abc; + double[] rot=wk_arModifyMatrix_rot; + + arGetAngle(abc);//arGetAngle( rot, &a, &b, &c ); + a2 = abc[0]; + b2 = abc[1]; + c2 = abc[2]; + factor = 10.0*Math.PI/180.0; + + nyatla_arGetNewMatrix_row3(trans,combo);//comboの3行目を先に計算 + for( j = 0; j < 10; j++ ) { + minerr = 1000000000.0; + for(t1=-1;t1<=1;t1++) { + a1 = a2 + factor*t1; + arGetRotA(a1); + for(t2=-1;t2<=1;t2++) { + b1 = b2 + factor*t2; + arGetRotB(b1,rot); + for(t3=-1;t3<=1;t3++) { + c1 = c2 + factor*t3; + arGetRotC(c1,rot); + //comboの0-2行目を計算 + nyatla_arGetNewMatrix_row012(rot,trans,combo);//第二パラメタは常にnull//arGetNewMatrix(trans, null, combo ); + err = 0.0; + for( i = 0; i < num; i++ ) { + hx = combo[0] * vertex[i][0]+ combo[1] * vertex[i][1]+ combo[2] * vertex[i][2]+ combo[3]; + hy = combo[4] * vertex[i][0]+ combo[5] * vertex[i][1]+ combo[6] * vertex[i][2]+ combo[7]; + h = combo[8] * vertex[i][0]+ combo[9] * vertex[i][1]+ combo[10] * vertex[i][2]+ combo[11]; + x = hx / h; + y = hy / h; + err += (pos2d[i][0] - x) * (pos2d[i][0] - x)+ (pos2d[i][1] - y) * (pos2d[i][1] - y); + } + if( err < minerr ) { + minerr = err; + ma = a1; + mb = b1; + mc = c1; + s1 = t1; + s2 = t2; + s3 = t3; + } + } + } + } + if( s1 == 0 && s2 == 0 && s3 == 0 ){ + factor *= 0.5; + } + a2 = ma; + b2 = mb; + c2 = mc; + } + arGetRot(ma,mb,mc,this.array); + /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ + return minerr/num; + } + /** + * arGetNewMatrixの0-2行目を初期化する関数 + * Optimize:2008.04.20:STEP[569->144] + * @param i_rot + * @param trans + * @param trans2 + * @param ret + * double[3x4]配列 + * @return + */ + private final void nyatla_arGetNewMatrix_row012(double i_rot[],double trans[],double ret[]) throws NyARException + { + int j; + double c0,c1,c2; + final double cpara2[]=cparam.get34Array(); + for( j = 0; j < 3; j++ ) { + //cpara2_pt=cpara2[j]; + c0=cpara2[j*4+0]; + c1=cpara2[j*4+1]; + c2=cpara2[j*4+2]; + ret[j*4+0] = c0 * i_rot[0]+ c1 * i_rot[3]+ c2 * i_rot[6]; + ret[j*4+1] = c0 * i_rot[1]+ c1 * i_rot[4]+ c2 * i_rot[7]; + ret[j*4+2] = c0 * i_rot[2]+ c1 * i_rot[5]+ c2 * i_rot[8]; + // + } + return; + } + /** + * arGetNewMatrixの3行目を初期化する関数 + * @param trans + * @param ret + * @throws NyARException + */ + private final void nyatla_arGetNewMatrix_row3(double trans[],double ret[]) throws NyARException + { + final double cpara2[]=cparam.get34Array(); + int j,j_idx; + for( j = 0; j < 3; j++ ) { + j_idx=j*4; + ret[j_idx+3] = cpara2[j_idx+0] * trans[0]+ cpara2[j_idx+1] * trans[1]+ cpara2[j_idx+2] * trans[2]+ cpara2[j_idx+3]; + } + return; + } +} + + +/** + * NyARModifyMatrixの最適化バージョン3 + * O3版の演算テーブル版 + * 計算速度のみを追求する + * + */ +class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon +{ + public NyARTransRot_O3(NyARParam i_param,int i_number_of_vertex) throws NyARException + { + super(i_param,i_number_of_vertex); + if(i_number_of_vertex!=4){ + //4以外の頂点数は処理しない + throw new NyARException(); + } + } + + //private double CACA,SASA,SACA,CA,SA; + private final double[][] wk_initRot_wdir=new double[3][3]; + /** + * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double rot[3][3] ) + * Optimize:2008.04.20:STEP[716→698] + * @param marker_info + * @param i_direction + * @param i_param + * @throws NyARException + */ + public void initRot(NyARSquare marker_info,int i_direction) throws NyARException + { + double cpara[]= cparam.get34Array(); + double[][] wdir=wk_initRot_wdir;//この関数で初期化される + double w, w1, w2, w3; + int dir; + int j; + + dir = i_direction; + + for( j = 0; j < 2; j++ ) { + w1 = marker_info.line[(4-dir+j)%4][0] * marker_info.line[(6-dir+j)%4][1]- marker_info.line[(6-dir+j)%4][0] * marker_info.line[(4-dir+j)%4][1]; + w2 = marker_info.line[(4-dir+j)%4][1] * marker_info.line[(6-dir+j)%4][2]- marker_info.line[(6-dir+j)%4][1] * marker_info.line[(4-dir+j)%4][2]; + w3 = marker_info.line[(4-dir+j)%4][2] * marker_info.line[(6-dir+j)%4][0]- marker_info.line[(6-dir+j)%4][2] * marker_info.line[(4-dir+j)%4][0]; + + wdir[j][0] = w1*(cpara[0*4+1]*cpara[1*4+2]-cpara[0*4+2]*cpara[1*4+1])+ w2*cpara[1*4+1]- w3*cpara[0*4+1]; + wdir[j][1] = -w1*cpara[0*4+0]*cpara[1*4+2]+ w3*cpara[0*4+0]; + wdir[j][2] = w1*cpara[0*4+0]*cpara[1*4+1]; + w = Math.sqrt( wdir[j][0]*wdir[j][0]+ wdir[j][1]*wdir[j][1]+ wdir[j][2]*wdir[j][2] ); + wdir[j][0] /= w; + wdir[j][1] /= w; + wdir[j][2] /= w; + } + + //以下3ケースは、計算エラーのときは例外が発生する。 + check_dir(wdir[0], marker_info.sqvertex[(4-dir)%4],marker_info.sqvertex[(5-dir)%4], cpara); + + check_dir(wdir[1], marker_info.sqvertex[(7-dir)%4],marker_info.sqvertex[(4-dir)%4], cpara); + + check_rotation(wdir); + + + wdir[2][0] = wdir[0][1]*wdir[1][2] - wdir[0][2]*wdir[1][1]; + wdir[2][1] = wdir[0][2]*wdir[1][0] - wdir[0][0]*wdir[1][2]; + wdir[2][2] = wdir[0][0]*wdir[1][1] - wdir[0][1]*wdir[1][0]; + w = Math.sqrt( wdir[2][0]*wdir[2][0]+ wdir[2][1]*wdir[2][1]+ wdir[2][2]*wdir[2][2] ); + wdir[2][0] /= w; + wdir[2][1] /= w; + wdir[2][2] /= w; + double[] rot=this.array; + rot[0] = wdir[0][0]; + rot[3] = wdir[0][1]; + rot[6] = wdir[0][2]; + rot[1] = wdir[1][0]; + rot[4] = wdir[1][1]; + rot[7] = wdir[1][2]; + rot[2] = wdir[2][0]; + rot[5] = wdir[2][1]; + rot[8] = wdir[2][2]; + // + } + private final double[][] wk_arModifyMatrix_double1D=new double[8][3]; + /** + * arGetRot計算を階層化したModifyMatrix + * 896 + * @param nyrot + * @param trans + * @param vertex + * [m][3] + * @param pos2d + * [n][2] + * @return + * @throws NyARException + */ + public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException + { + double factor; + double a2, b2, c2; + double ma = 0.0, mb = 0.0, mc = 0.0; + double h, x, y; + double err, minerr=0; + int t1, t2, t3; + int s1 = 0, s2 = 0, s3 = 0; + + factor = 10.0*Math.PI/180.0; + double rot0,rot1,rot3,rot4,rot6,rot7; + double combo00,combo01,combo02,combo03,combo10,combo11,combo12,combo13,combo20,combo21,combo22,combo23; + double combo02_2,combo02_5,combo02_8,combo02_11; + double combo22_2,combo22_5,combo22_8,combo22_11; + double combo12_2,combo12_5,combo12_8,combo12_11; + //vertex展開 + final double VX00,VX01,VX02,VX10,VX11,VX12,VX20,VX21,VX22,VX30,VX31,VX32; + double[] d_pt; + d_pt=vertex[0];VX00=d_pt[0];VX01=d_pt[1];VX02=d_pt[2]; + d_pt=vertex[1];VX10=d_pt[0];VX11=d_pt[1];VX12=d_pt[2]; + d_pt=vertex[2];VX20=d_pt[0];VX21=d_pt[1];VX22=d_pt[2]; + d_pt=vertex[3];VX30=d_pt[0];VX31=d_pt[1];VX32=d_pt[2]; + final double P2D00,P2D01,P2D10,P2D11,P2D20,P2D21,P2D30,P2D31; + d_pt=pos2d[0];P2D00=d_pt[0];P2D01=d_pt[1]; + d_pt=pos2d[1];P2D10=d_pt[0];P2D11=d_pt[1]; + d_pt=pos2d[2];P2D20=d_pt[0];P2D21=d_pt[1]; + d_pt=pos2d[3];P2D30=d_pt[0];P2D31=d_pt[1]; + final double cpara[]=cparam.get34Array(); + final double CP0,CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10; + CP0=cpara[0];CP1=cpara[1];CP2=cpara[2];CP3=cpara[3]; + CP4=cpara[4];CP5=cpara[5];CP6=cpara[6];CP7=cpara[7]; + CP8=cpara[8];CP9=cpara[9];CP10=cpara[10]; + combo03 = CP0 * trans[0]+ CP1 * trans[1]+ CP2 * trans[2]+ CP3; + combo13 = CP4 * trans[0]+ CP5 * trans[1]+ CP6 * trans[2]+ CP7; + combo23 = CP8 * trans[0]+ CP9 * trans[1]+ CP10 * trans[2]+ cpara[11]; + double CACA,SASA,SACA,CA,SA; + double CACACB,SACACB,SASACB,CASB,SASB; + double SACASC,SACACBSC,SACACBCC,SACACC; + final double[][] double1D=this.wk_arModifyMatrix_double1D; + + final double[] abc =double1D[0]; + final double[] a_factor=double1D[1]; + final double[] sinb =double1D[2]; + final double[] cosb =double1D[3]; + final double[] b_factor=double1D[4]; + final double[] sinc =double1D[5]; + final double[] cosc =double1D[6]; + final double[] c_factor=double1D[7]; + double w,w2; + double wsin,wcos; + + arGetAngle(abc);//arGetAngle( rot, &a, &b, &c ); + a2 = abc[0]; + b2 = abc[1]; + c2 = abc[2]; + + //comboの3行目を先に計算 + for(int i = 0; i < 10; i++ ) { + minerr = 1000000000.0; + //sin-cosテーブルを計算(これが外に出せるとは…。) + for(int j=0;j<3;j++){ + w2=factor*(j-1); + w= a2 + w2; + a_factor[j]=w; + w= b2 + w2; + b_factor[j]=w; + sinb[j]=Math.sin(w); + cosb[j]=Math.cos(w); + w= c2 + w2; + c_factor[j]=w; + sinc[j]=Math.sin(w); + cosc[j]=Math.cos(w); + } + // + for(t1=0;t1<3;t1++) { + SA = Math.sin(a_factor[t1]); + CA = Math.cos(a_factor[t1]); + //Optimize + CACA=CA*CA; + SASA=SA*SA; + SACA=SA*CA; + for(t2=0;t2<3;t2++) { + wsin=sinb[t2]; + wcos=cosb[t2]; + CACACB=CACA*wcos; + SACACB=SACA*wcos; + SASACB=SASA*wcos; + CASB=CA*wsin; + SASB=SA*wsin; + //comboの計算1 + combo02 = CP0 * CASB+ CP1 * SASB+ CP2 * wcos; + combo12 = CP4 * CASB+ CP5 * SASB+ CP6 * wcos; + combo22 = CP8 * CASB+ CP9 * SASB+ CP10 * wcos; + + combo02_2 =combo02 * VX02 + combo03; + combo02_5 =combo02 * VX12 + combo03; + combo02_8 =combo02 * VX22 + combo03; + combo02_11=combo02 * VX32 + combo03; + combo12_2 =combo12 * VX02 + combo13; + combo12_5 =combo12 * VX12 + combo13; + combo12_8 =combo12 * VX22 + combo13; + combo12_11=combo12 * VX32 + combo13; + combo22_2 =combo22 * VX02 + combo23; + combo22_5 =combo22 * VX12 + combo23; + combo22_8 =combo22 * VX22 + combo23; + combo22_11=combo22 * VX32 + combo23; + for(t3=0;t3<3;t3++){ + wsin=sinc[t3]; + wcos=cosc[t3]; + SACASC=SACA*wsin; + SACACC=SACA*wcos; + SACACBSC=SACACB*wsin; + SACACBCC=SACACB*wcos; + + rot0 = CACACB*wcos+SASA*wcos+SACACBSC-SACASC; + rot3 = SACACBCC-SACACC+SASACB*wsin+CACA*wsin; + rot6 = -CASB*wcos-SASB*wsin; + + combo00 = CP0 * rot0+ CP1 * rot3+ CP2 * rot6; + combo10 = CP4 * rot0+ CP5 * rot3+ CP6 * rot6; + combo20 = CP8 * rot0+ CP9 * rot3+ CP10 * rot6; + + rot1 = -CACACB*wsin-SASA*wsin+SACACBCC-SACACC; + rot4 = -SACACBSC+SACASC+SASACB*wcos+CACA*wcos; + rot7 = CASB*wsin-SASB*wcos; + combo01 = CP0 * rot1+ CP1 * rot4+ CP2 * rot7; + combo11 = CP4 * rot1+ CP5 * rot4+ CP6 * rot7; + combo21 = CP8 * rot1+ CP9 * rot4+ CP10 * rot7; + // + err = 0.0; + h = combo20 * VX00+ combo21 * VX01+ combo22_2; + x = P2D00 - (combo00 * VX00+ combo01 * VX01+ combo02_2) / h; + y = P2D01 - (combo10 * VX00+ combo11 * VX01+ combo12_2) / h; + err += x*x+y*y; + h = combo20 * VX10+ combo21 * VX11+ combo22_5; + x = P2D10 - (combo00 * VX10+ combo01 * VX11+ combo02_5) / h; + y = P2D11 - (combo10 * VX10+ combo11 * VX11+ combo12_5) / h; + err += x*x+y*y; + h = combo20 * VX20+ combo21 * VX21+ combo22_8; + x = P2D20 - (combo00 * VX20+ combo01 * VX21+ combo02_8) / h; + y = P2D21 - (combo10 * VX20+ combo11 * VX21+ combo12_8) / h; + err += x*x+y*y; + h = combo20 * VX30+ combo21 * VX31+ combo22_11; + x = P2D30 - (combo00 * VX30+ combo01 * VX31+ combo02_11) / h; + y = P2D31 - (combo10 * VX30+ combo11 * VX31+ combo12_11) / h; + err += x*x+y*y; + if( err < minerr ) { + minerr = err; + ma = a_factor[t1]; + mb = b_factor[t2]; + mc = c_factor[t3]; + s1 = t1-1; + s2 = t2-1; + s3 = t3-1; + } + } + } + } + if( s1 == 0 && s2 == 0 && s3 == 0 ){ + factor *= 0.5; + } + a2 = ma; + b2 = mb; + c2 = mc; + } + arGetRot(ma,mb,mc,this.array); + /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ + return minerr/4; + } +} + diff --git a/src/jp/nyatla/nyartoolkit/core/NyARVec.java b/src/jp/nyatla/nyartoolkit/core/NyARVec.java new file mode 100644 index 0000000..66b81f2 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARVec.java @@ -0,0 +1,260 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + +import jp.nyatla.nyartoolkit.NyARException; + + + +public class NyARVec +{ + private int clm; + public NyARVec(int i_clm) + { + v=new double[i_clm]; + clm=i_clm; + } + private double[] v; + /** + * i_clmサイズの列を格納できるように列サイズを変更します。 + * 実行後、列の各値は不定になります。 + * @param i_clm + */ + public void realloc(int i_clm) + { + if(i_clm<=this.v.length) + { + //十分な配列があれば何もしない。 + }else{ + //不十分なら取り直す。 + v=new double[i_clm]; + } + this.clm=i_clm; + } + public int getClm() + { + return clm; + } + public double[] getArray() + { + return v; + } + /** + * arVecDispの代替品 + * @param value + * @return + */ + public int arVecDisp() throws NyARException + { + NyARException.trap("未チェックのパス"); + System.out.println(" === vector ("+clm+") ===\n");//printf(" === vector (%d) ===\n", v->clm); + System.out.print(" |");//printf(" |"); + for(int c = 0; c < clm; c++ ){//for( c = 0; c < v->clm; c++ ){ + System.out.print(" "+v[c]);//printf( " %10g", v->v[c] ); + } + System.out.println(" |");//printf(" |\n"); + System.out.println(" ===================");//printf(" ===================\n"); + return 0; + } + /** + * arVecInnerproduct関数の代替品 + * @param x + * @param y + * @param i_start + * 演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる) + * @return + * @throws NyARException + */ + public double vecInnerproduct(NyARVec y,int i_start) throws NyARException + { + NyARException.trap("この関数は動作確認できていません。"); + double result = 0.0; +// double[] x_array=x.v;.getArray(); +// double[] y_array=y.getArray(); + + if(this.clm!= y.clm){ + throw new NyARException();//exit(); + } + for(int i = i_start; i < this.clm; i++ ) { + NyARException.trap("未チェックのパス"); + result += this.v[i] * y.v[i];//result += x->v[i] * y->v[i]; + } + return result; + } + /** + * double arVecHousehold関数の代替品 + * @param x + * @param i_start + * 演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる) + * @return + * @throws NyARException + */ + public double vecHousehold(int i_start) throws NyARException + { + NyARException.trap("この関数は動作確認できていません。"); + double s, t; + s = Math.sqrt(this.vecInnerproduct(this,i_start)); +// double[] x_array=x.getArray(); + if( s != 0.0 ){ + NyARException.trap("未チェックのパス"); + if(this.v[i_start]< 0){ + s = -s; + } + NyARException.trap("未チェックのパス");{ + this.v[i_start]+=s;//x->v[0] += s; + t = 1 / Math.sqrt(this.v[i_start]* s);//t = 1 / sqrt(x->v[0] * s); + } + for(int i = i_start; i < this.clm; i++){ + NyARException.trap("未チェックのパス"); + this.v[i]*=t;//x->v[i] *= t; + } + } + return -s; + } +// /** +// * arVecTridiagonalize関数の代替品 +// * a,d,e間で演算をしてる。何をどうしているかはさっぱりさっぱり +// * @param a +// * @param d +// * @param e +// * @param i_e_start +// * 演算開始列(よくわからないけどarVecTridiagonalizeの呼び出し元でなんかしてる) +// * @return +// * @throws NyARException +// */ +// public static void vecTridiagonalize(NyARMat a, NyARVec d, NyARVec e,int i_e_start) throws NyARException +// { +// NyARVec vec,vec2; +// double[][] a_array=a.getArray(); +// double s, t, p, q; +// int dim; +// +// if(a.getClm()!=a.getRow()){ +// throw new NyARException(); +// } +// if(a.getClm() != d.clm){ +// throw new NyARException(); +// } +// if(a.getClm() != e.clm){ +// throw new NyARException(); +// } +// dim = a.getClm(); +// +// for(int k = 0; k < dim-2; k++ ){ +// vec=a.getRowVec(k); +//// double[] vec_array=vec.getArray(); +// NyARException.trap("未チェックパス"); +// d.v[k]=vec.v[k];//d.set(k,v.get(k)); //d->v[k] = v[k]; +// +// //wv1.clm = dim-k-1; +// //wv1.v = &(v[k+1]); +// NyARException.trap("未チェックパス"); +// e.v[k+i_e_start]=vec.vecHousehold(k+1);//e->v[k] = arVecHousehold(&wv1); +// if(e.v[k+i_e_start]== 0.0 ){ +// continue; +// } +// +// for(int i = k+1; i < dim; i++ ){ +// s = 0.0; +// for(int j = k+1; j < i; j++ ) { +// NyARException.trap("未チェックのパス"); +// s += a_array[j][i] * vec.v[j];//s += a.get(j*dim+i) * v.get(j);//s += a->m[j*dim+i] * v[j]; +// } +// for(int j = i; j < dim; j++ ) { +// NyARException.trap("未チェックのパス"); +// s += a_array[i][j] * vec.v[j];//s += a.get(i*dim+j) * v.get(j);//s += a->m[i*dim+j] * v[j]; +// } +// NyARException.trap("未チェックのパス"); +// d.v[i]=s;//d->v[i] = s; +// } +// +// +// //wv1.clm = wv2.clm = dim-k-1; +// //wv1.v = &(v[k+1]); +// //wv2.v = &(d->v[k+1]); +// vec=a.getRowVec(k); +//// vec_array=vec.getArray(); +// NyARException.trap("未チェックパス"); +// t = vec.vecInnerproduct(d,k+1)/ 2; +// for(int i = dim-1; i > k; i-- ) { +// NyARException.trap("未チェックパス"); +// p = vec.v[i];//p = v.get(i);//p = v[i]; +// d.v[i]-=t*p;q=d.v[i];//q = d->v[i] -= t*p； +// for(int j = i; j < dim; j++ ){ +// NyARException.trap("未チェックパス"); +// a_array[i][j]-=p*(d.v[j] + q*vec.v[j]);//a->m[i*dim+j] -= p*(d->v[j]) + q*v[j]; +// } +// } +// } +// +// if( dim >= 2) { +// d.v[dim-2]=a_array[dim-2][dim-2];//d->v[dim-2] = a->m[(dim-2)*dim+(dim-2)]; +// e.v[dim-2+i_e_start]=a_array[dim-2][dim-1];//e->v[dim-2] = a->m[(dim-2)*dim+(dim-1)]; +// } +// +// if( dim >= 1 ){ +// d.v[dim-1]=a_array[dim-1][dim-1];//d->v[dim-1] = a->m[(dim-1)*dim+(dim-1)]; +// } +// +// for(int k = dim-1; k >= 0; k--) { +// vec=a.getRowVec(k);//v = a.getPointer(k*dim);//v = &(a->m[k*dim]); +// if( k < dim-2 ) { +// for(int i = k+1; i < dim; i++ ){ +// //wv1.clm = wv2.clm = dim-k-1; +// //wv1.v = &(v[k+1]); +// //wv2.v = &(a->m[i*dim+k+1]); +// vec2=a.getRowVec(i); +// +// t = vec.vecInnerproduct(vec2,k+1); +// for(int j = k+1; j < dim; j++ ){ +// NyARException.trap("未チェックパス"); +// a_array[i][j]-=t*vec.v[j];//a.subValue(i*dim+j,t*v.get(j));//a->m[i*dim+j] -= t * v[j]; +// } +// } +// } +// for(int i = 0; i < dim; i++ ){ +// vec.v[i]=0.0;//v.set(i,0.0);//v[i] = 0.0; +// } +// vec.v[k]=1;//v.set(k,1);//v[k] = 1; +// } +// } + /** + * 現在ラップしている配列を取り外して、新しい配列をラップします。 + * @param i_v + * @param i_clm + */ + public void setNewArray(double[] i_array,int i_clm) + { + this.v=i_array; + this.clm=i_clm; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/NyARVersion.java b/src/jp/nyatla/nyartoolkit/core/NyARVersion.java new file mode 100644 index 0000000..ef76572 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/NyARVersion.java @@ -0,0 +1,66 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core; + + +/** + * ARUint32 arGetVersion(char **versionStringRef); + * 関数の置き換え + */ +public class NyARVersion { + private static final int AR_HEADER_VERSION_MAJOR=2; //#define AR_HEADER_VERSION_MAJOR 2 + private static final int AR_HEADER_VERSION_MINOR=72;//#define AR_HEADER_VERSION_MINOR 72 + private static final int AR_HEADER_VERSION_TINY=0;//#define AR_HEADER_VERSION_TINY 0 + private static final int AR_HEADER_VERSION_BUILD=0;//#define AR_HEADER_VERSION_BUILD 0 + private static final String AR_HEADER_VERSION_STRING="2.72.0";//#define AR_HEADER_VERSION_STRING "2.72.0" + public static final boolean AR_HAVE_HEADER_VERSION_2=true;//#define AR_HAVE_HEADER_VERSION_2 + public static final boolean AR_HAVE_HEADER_VERSION_2_72=true;//#define AR_HAVE_HEADER_VERSION_2_72 + + public static String getARVersion() + { + return AR_HEADER_VERSION_STRING; + } + public static int getARVersionInt() + { + // Represent full version number (major, minor, tiny, build) in + // binary coded decimal. N.B: Integer division. + return (int)(0x10000000 * (AR_HEADER_VERSION_MAJOR / 10)) + + (int)(0x01000000 * (AR_HEADER_VERSION_MAJOR % 10)) + + (int)(0x00100000 * (AR_HEADER_VERSION_MINOR / 10)) + + (int)(0x00010000 * (AR_HEADER_VERSION_MINOR % 10)) + + (int)(0x00001000 * (AR_HEADER_VERSION_TINY / 10)) + + (int)(0x00000100 * (AR_HEADER_VERSION_TINY % 10)) + + (int)(0x00000010 * (AR_HEADER_VERSION_BUILD / 10)) + + (int)(0x00000001 * (AR_HEADER_VERSION_BUILD % 10)); + + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt.java b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt.java new file mode 100644 index 0000000..c939985 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt.java @@ -0,0 +1,59 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core.match; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.NyARCode; +import jp.nyatla.nyartoolkit.core.NyARColorPatt; + + + + + +/** + * ARColorPattのマッチング計算をするインタフェイスです。 + * 基準Patに対して、計算済みのARCodeデータとの間で比較演算をします。 + * pattern_match関数を分解した３種類のパターン検出クラスを定義します。 + * + */ +public interface NyARMatchPatt{ + public double getConfidence(); + public int getDirection(); + public void evaluate(NyARCode i_code); + public boolean setPatt(NyARColorPatt i_target_patt) throws NyARException; +} + + + + + + diff --git a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_BlackWhite.java b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_BlackWhite.java new file mode 100644 index 0000000..6c2450c --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_BlackWhite.java @@ -0,0 +1,113 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core.match; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.*; + +/** + * AR_TEMPLATE_MATCHING_BWと同等のルールで + * マーカーを評価します。 + * + */ +public class NyARMatchPatt_BlackWhite implements NyARMatchPatt{ + private double datapow; + private int width; + private int height; + private double cf=0; + private int dir=0; + private int ave; + private int[][][] input=new int[height][width][3]; + public boolean setPatt(NyARColorPatt i_target_patt) throws NyARException + { + width=i_target_patt.getWidth(); + height=i_target_patt.getHeight(); + int[][][] data=i_target_patt.getPatArray(); + input=new int[height][width][3]; + + int sum = ave = 0; + for(int i=0;i max ) { + max = sum2; + res = j; + } + } + dir=res; + cf=max; + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITHOUT_PCA.java b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITHOUT_PCA.java new file mode 100644 index 0000000..6bb033d --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITHOUT_PCA.java @@ -0,0 +1,170 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core.match; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.*; + +/** + * AR_TEMPLATE_MATCHING_COLORかつAR_MATCHING_WITHOUT_PCAと同等のルールで + * マーカーを評価します。 + * + */ +public class NyARMatchPatt_Color_WITHOUT_PCA implements NyARMatchPatt{ + private int[][][] input=new int[1][1][3]; + private double datapow; + + private int width =1; + private int height=1; + private double cf=0; + private int dir=0; + public double getConfidence(){ + return cf; + } + public int getDirection(){ + return dir; + } + /** + * input配列サイズを必要に応じて再アロケートする。 + * + * @param i_width + * @param i_height + */ + private void reallocInputArray(int i_width,int i_height) + { + if(this.input.length=0;i--){//for(int i=0;i=0;k--) {//for(int i2=0;i2l_ave += (255-data[i][i2][0])+(255-data[i][i2][1])+(255-data[i][i2][2]); + data_i_k=data_i[k]; + l_ave += 255*3-data_i_k[0]-data_i_k[1]-data_i_k[2]; + } + } + l_ave /= (lheight*lwidth*3); + for(i=lheight-1;i>=0;i--){//for(i=0;i=0;k--){//for(i2=0;i2 + //for(int i3=0;i3<3;i3++){ + // input[i][i2][i3] = (255-data[i][i2][i3]) - l_ave; + // sum += input[i][i2][i3]*input[i][i2][i3]; + //} + data_i_k =data_i[k]; + input_i_k=input_i[k]; + w_sum=(255-data_i_k[0]) - l_ave; + input_i_k[0]=w_sum; + sum += w_sum*w_sum; + + w_sum=(255-data_i_k[1]) - l_ave; + input_i_k[1]=w_sum; + sum += w_sum*w_sum; + + w_sum=(255-data_i_k[2]) - l_ave; + input_i_k[2]=w_sum; + sum+=w_sum*w_sum; + // + } + } + datapow = Math.sqrt( (double)sum ); + if(datapow == 0.0){ + return false;// throw new NyARException(); +// dir.set(0);//*dir = 0; +// cf.set(-1.0);//*cf = -1.0; +// return -1; + } + return true; + } + /** + * public int pattern_match(short[][][] data,IntPointer dir,DoublePointer cf) + + */ + public void evaluate(NyARCode i_code) + { + int[][][][] pat=i_code.getPat(); + double[] patpow=i_code.getPatPow(); + int res= -1; + double max=0.0; + int[][][] pat_j,linput; + int[][] pat_j_i,input_i; + int[] pat_j_i_k,input_i_k; + int l_width=this.width; + int l_height=this.height; + linput=this.input; + for(int j = 0; j < 4; j++ ) { + int sum = 0; + pat_j=pat[j]; + for(int i=l_height-1;i>=0;i--){//for(int i=0;i=0;k--){ + pat_j_i_k=pat_j_i[k]; + input_i_k=input_i[k]; +// for(int i3=0;i3<3;i3++){ + sum += input_i_k[0]*pat_j_i_k[0];//sum += input[i][i2][i3]*pat[k][j][i][i2][i3]; + sum += input_i_k[1]*pat_j_i_k[1];//sum += input[i][i2][i3]*pat[k][j][i][i2][i3]; + sum += input_i_k[2]*pat_j_i_k[2];//sum += input[i][i2][i3]*pat[k][j][i][i2][i3]; +// } + } + } + double sum2 = sum / patpow[j] / datapow;//sum2 = sum / patpow[k][j] / datapow; + if( sum2 > max ){ + max = sum2; + res = j; + } + } + dir=res; + cf=max; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITH_PCA.java b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITH_PCA.java new file mode 100644 index 0000000..1f387b6 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/match/NyARMatchPatt_Color_WITH_PCA.java @@ -0,0 +1,147 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core.match; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.NyARCode; +import jp.nyatla.nyartoolkit.core.NyARColorPatt; + +/** + * AR_TEMPLATE_MATCHING_COLORかつAR_MATCHING_WITH_PCAと同等のルールで + * マーカーを評価します。 + * + */ +public class NyARMatchPatt_Color_WITH_PCA implements NyARMatchPatt{ + private final int EVEC_MAX=10;//#define EVEC_MAX 10 + private int evec_dim;//static int evec_dim; + private int[][][] input; + private double[][][][] evec;//static double evec[EVEC_MAX][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3]; + private double[][] epat=new double[4][EVEC_MAX];//static double epat[AR_PATT_NUM_MAX][4][EVEC_MAX]; + private int ave; + private double datapow; + + private int width; + private int height; + private double cf=0; + private int dir=0;//向きか！ + public double getConfidence(){ + return cf; + } + public int getDirection(){ + return dir; + } + public boolean setPatt(NyARColorPatt i_target_patt) throws NyARException + { + width=i_target_patt.getWidth(); + height=i_target_patt.getHeight(); + int[][][] data=i_target_patt.getPatArray(); + + input=new int[height][width][3]; + evec=new double[EVEC_MAX][height][width][3];//static double evec[EVEC_MAX][AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3]; + int sum; + + sum = ave = 0; + for(int i=0;i + * + */ +package jp.nyatla.nyartoolkit.core.raster; + +public interface NyARRaster{ + //RGBの合計値を返す + public int getPixelTotal(int i_x,int i_y); + /** + * 一行単位でi_row番目の合計値配列を計算して返す。 + * @param i_row + * @param o_line + * getWidth()の戻り値以上のサイズが必要。 + */ + public void getPixelTotalRowLine(int i_row,int[] o_line); + public int getWidth(); + public int getHeight(); + public void getPixel(int i_x,int i_y,int[] i_rgb); + /** + * 複数のピクセル値をi_rgbへ返します。 + * @param i_x + * xのインデックス配列 + * @param i_y + * yのインデックス配列 + * @param i_num + * 返すピクセル値の数 + * @param i_rgb + * ピクセル値を返すバッファ + */ + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb); +} + + + diff --git a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_BGRA.java b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_BGRA.java new file mode 100644 index 0000000..01bcfc6 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_BGRA.java @@ -0,0 +1,93 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core.raster; + + +public class NyARRaster_BGRA implements NyARRaster +{ + private byte[] ref_buf; + private int width; + private int height; + public static NyARRaster_BGRA wrap(byte[] i_buffer,int i_width,int i_height) + { + NyARRaster_BGRA new_inst=new NyARRaster_BGRA(); + new_inst.ref_buf=i_buffer; + new_inst.width =i_width; + new_inst.height =i_height; + return new_inst; + } + //RGBの合計値を返す + public int getPixelTotal(int i_x,int i_y) + { + int bp=(i_x+i_y*this.width)*4; + byte[] ref=this.ref_buf; + return (ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); + } + public void getPixelTotalRowLine(int i_row,int[] o_line) + { + final byte[] ref=this.ref_buf; + int bp=(i_row+1)*this.width*4-4; + for(int i=this.width-1;i>=0;i--){ + o_line[i]=(ref[bp] & 0xff)+(ref[bp+1] & 0xff)+(ref[bp+2] & 0xff); + bp-=4; + } + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + public void getPixel(int i_x,int i_y,int[] i_rgb) + { + byte[] ref=this.ref_buf; + int bp=(i_x+i_y*this.width)*4; + i_rgb[0]=(ref[bp+2] & 0xff);//R + i_rgb[1]=(ref[bp+1] & 0xff);//G + i_rgb[2]=(ref[bp+0] & 0xff);//B + } + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb) + { + int width=this.width; + byte[] ref=this.ref_buf; + int bp; + for(int i=i_num-1;i>=0;i--){ + bp=(i_x[i]+i_y[i]*width)*4; + o_rgb[i*3+0]=(ref[bp+2] & 0xff);//R + o_rgb[i*3+1]=(ref[bp+1] & 0xff);//G + o_rgb[i*3+2]=(ref[bp+0] & 0xff);//B + } + } +} + diff --git a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_Blank.java b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_Blank.java new file mode 100644 index 0000000..1bea194 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_Blank.java @@ -0,0 +1,81 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core.raster; + + +/* + * 真っ黒の矩形を定義する。 + * + */ +public class NyARRaster_Blank implements NyARRaster +{ + private int width; + private int height; + public NyARRaster_Blank(int i_width,int i_height) + { + width =i_width; + height =i_height; + } + //RGBの合計値を返す + public int getPixelTotal(int i_x,int i_y) + { + return 0; + } + public void getPixelTotalRowLine(int i_row,int[] o_line) + { + for(int i=this.width-1;i>=0;i--){ + o_line[i]=0; + } + } + public int getWidth() + { + return width; + } + public int getHeight() + { + return height; + } + public void getPixel(int i_x,int i_y,int[] i_rgb) + { + i_rgb[0]=0; + i_rgb[1]=0; + i_rgb[2]=0; + } + public void getPixelSet(int[] i_x,int i_y[],int i_num,int[] o_rgb) + { + for(int i=i_num-1;i>=0;i--){ + o_rgb[i*3+0]=0;//R + o_rgb[i*3+1]=0;//G + o_rgb[i*3+2]=0;//B + } + } +} diff --git a/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_RGB.java b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_RGB.java new file mode 100644 index 0000000..9a9c355 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/core/raster/NyARRaster_RGB.java @@ -0,0 +1,99 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.core.raster; + +public class NyARRaster_RGB implements NyARRaster { + protected byte[] ref_buf; + protected int width; + protected int height; + + public static NyARRaster_RGB wrap(byte[] i_buffer, int i_width, int i_height) { + NyARRaster_RGB new_inst = new NyARRaster_RGB(); + new_inst.ref_buf = i_buffer; + new_inst.width = i_width; + new_inst.height = i_height; + return new_inst; + } + + public static NyARRaster_RGB wrap(NyARRaster_RGB new_inst, byte[] i_buffer, + int i_width, int i_height) { + new_inst.ref_buf = i_buffer; + new_inst.width = i_width; + new_inst.height = i_height; + return new_inst; + } + + // RGBの合計値を返す + public int getPixelTotal(int i_x, int i_y) { + byte[] ref = this.ref_buf; + int bp = (i_x + i_y * this.width) * 3; + return (ref[bp] & 0xff) + (ref[bp + 1] & 0xff) + (ref[bp + 2] & 0xff); + } + + public void getPixelTotalRowLine(int i_row, int[] o_line) { + final byte[] ref = this.ref_buf; + int bp = (i_row + 1) * this.width * 3 - 3; + for (int i = this.width - 1; i >= 0; i--) { + o_line[i] = (ref[bp] & 0xff) + (ref[bp + 1] & 0xff) + + (ref[bp + 2] & 0xff); + bp -= 3; + } + } + + public int getWidth() { + return width; + } + + public int getHeight() { + return height; + } + + public void getPixel(int i_x, int i_y, int[] i_rgb) { + int bp = (i_x + i_y * this.width) * 3; + byte[] ref = this.ref_buf; + i_rgb[0] = (ref[bp + 0] & 0xff);// R + i_rgb[1] = (ref[bp + 1] & 0xff);// G + i_rgb[2] = (ref[bp + 2] & 0xff);// B + } + + public void getPixelSet(int[] i_x, int i_y[], int i_num, int[] o_rgb) { + int width = this.width; + byte[] ref = this.ref_buf; + int bp; + for (int i = i_num - 1; i >= 0; i--) { + bp = (i_x[i] + i_y[i] * width) * 3; + o_rgb[i * 3 + 0] = (ref[bp + 0] & 0xff);// R + o_rgb[i * 3 + 1] = (ref[bp + 1] & 0xff);// G + o_rgb[i * 3 + 2] = (ref[bp + 2] & 0xff);// B + } + } +} diff --git a/src/jp/nyatla/nyartoolkit/detector/NyARDetectMarker.java b/src/jp/nyatla/nyartoolkit/detector/NyARDetectMarker.java new file mode 100644 index 0000000..fd477a0 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/detector/NyARDetectMarker.java @@ -0,0 +1,278 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.detector; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.*; +import jp.nyatla.nyartoolkit.core.match.NyARMatchPatt_Color_WITHOUT_PCA; +import jp.nyatla.nyartoolkit.core.raster.*; + + + +class NyARDetectMarkerResult +{ + public int arcode_id; + public int direction; + public double confidence; + public NyARSquare ref_square; +} +class NyARDetectMarkerResultHolder +{ + public NyARDetectMarkerResult[] result_array=new NyARDetectMarkerResult[1]; + /** + * result_holderを最大i_reserve_size個の要素を格納できるように予約します。 + * @param i_reserve_size + */ + public void reservHolder(int i_reserve_size) + { + if(i_reserve_size>=result_array.length){ + int new_size=i_reserve_size+5; + result_array=new NyARDetectMarkerResult[new_size]; + for(int i=0;ic2){ + continue; + } + //より一致するARCodeの情報を保存 + code_index =i2; + direction =match_patt.getDirection(); + confidence =c2; + } + //i番目のパターン情報を保存する。 + final NyARDetectMarkerResult result=this.result_holder.result_array[i]; + result.arcode_id =code_index; + result.confidence=confidence; + result.direction =direction; + result.ref_square=square; + } + return number_of_square; + } + /** + * i_indexのマーカーに対する変換行列を計算し、結果値をo_resultへ格納します。 + * 直前に実行したdetectMarkerLiteが成功していないと使えません。 + * @param i_index + * マーカーのインデックス番号を指定します。 + * 直前に実行したdetectMarkerLiteの戻り値未満かつ0以上である必要があります。 + * @param o_result + * 結果値を受け取るオブジェクトを指定してください。 + * @throws NyARException + */ + public void getTransmationMatrix(int i_index,NyARTransMatResult o_result) throws NyARException + { + final NyARDetectMarkerResult result=this.result_holder.result_array[i_index]; + //一番一致したマーカーの位置とかその辺を計算 + if(is_continue){ + transmat.transMatContinue(result.ref_square,result.direction,marker_width[result.arcode_id],o_result); + }else{ + transmat.transMat(result.ref_square,result.direction,marker_width[result.arcode_id],o_result); + } + return; + } + /** + * i_indexのマーカーの一致度を返します。 + * @param i_index + * マーカーのインデックス番号を指定します。 + * 直前に実行したdetectMarkerLiteの戻り値未満かつ0以上である必要があります。 + * @return + * マーカーの一致度を返します。0～1までの値をとります。 + * 一致度が低い場合には、誤認識の可能性が高くなります。 + * @throws NyARException + */ + public double getConfidence(int i_index) + { + return this.result_holder.result_array[i_index].confidence; + } + /** + * i_indexのマーカーの方位を返します。 + * @param i_index + * マーカーのインデックス番号を指定します。 + * 直前に実行したdetectMarkerLiteの戻り値未満かつ0以上である必要があります。 + * @return + * 0,1,2,3の何れかを返します。 + */ + public int getDirection(int i_index) + { + return this.result_holder.result_array[i_index].direction; + } + /** + * i_indexのマーカーのARCodeインデックスを返します。 + * @param i_index + * マーカーのインデックス番号を指定します。 + * 直前に実行したdetectMarkerLiteの戻り値未満かつ0以上である必要があります。 + * @return + */ + public int getARCodeIndex(int i_index) + { + return this.result_holder.result_array[i_index].arcode_id; + } + /** + * getTransmationMatrixの計算モードを設定します。 + * @param i_is_continue + * TRUEなら、transMatContinueを使用します。 + * FALSEなら、transMatを使用します。 + */ + public void setContinueMode(boolean i_is_continue) + { + this.is_continue=i_is_continue; + } + +} + + + + + + + + + + + + + + + diff --git a/src/jp/nyatla/nyartoolkit/detector/NyARSingleDetectMarker.java b/src/jp/nyatla/nyartoolkit/detector/NyARSingleDetectMarker.java new file mode 100644 index 0000000..8790ea9 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/detector/NyARSingleDetectMarker.java @@ -0,0 +1,203 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * This work is based on the original ARToolKit developed by + * Hirokazu Kato + * Mark Billinghurst + * HITLab, University of Washington, Seattle + * http://www.hitl.washington.edu/artoolkit/ + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.detector; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.*; +import jp.nyatla.nyartoolkit.core.match.NyARMatchPatt_Color_WITHOUT_PCA; +import jp.nyatla.nyartoolkit.core.raster.*; +/** + * 画像からARCodeに最も一致するマーカーを1個検出し、その変換行列を計算するクラスです。 + * + */ +public class NyARSingleDetectMarker{ + private static final int AR_SQUARE_MAX=100; + private boolean is_continue=false; + private NyARMatchPatt_Color_WITHOUT_PCA match_patt; + private NyARDetectSquare square; + private final NyARSquareList square_list=new NyARSquareList(AR_SQUARE_MAX); + private NyARCode code; + protected NyARTransMat transmat; + private double marker_width; + //検出結果の保存用 + private int detected_direction; + private double detected_confidence; + private NyARSquare detected_square; + private NyARColorPatt patt; + /** + * 検出するARCodeとカメラパラメータから、1個のARCodeを検出するNyARSingleDetectMarkerインスタンスを作ります。 + * @param i_param + * カメラパラメータを指定します。 + * @param i_code + * 検出するARCodeを指定します。 + * @param i_marker_width + * ARコードの物理サイズを、ミリメートルで指定します。 + * @throws NyARException + */ + public NyARSingleDetectMarker(NyARParam i_param,NyARCode i_code,double i_marker_width) throws NyARException + { + //解析オブジェクトを作る + this.square=new NyARDetectSquare(i_param); + this.transmat=new NyARTransMat_O2(i_param); + //比較コードを保存 + this.code=i_code; + this.marker_width=i_marker_width; + //評価パターンのホルダを作る + this.patt=new NyARColorPatt_O3(code.getWidth(),code.getHeight()); + //評価器を作る。 + this.match_patt=new NyARMatchPatt_Color_WITHOUT_PCA(); + } + /** + * i_imageにマーカー検出処理を実行し、結果を記録します。 + * @param i_image + * マーカーを検出するイメージを指定します。 + * @param i_thresh + * 検出閾値を指定します。0～255の範囲で指定してください。 + * 通常は100～130くらいを指定します。 + * @return + * マーカーが検出できたかを真偽値で返します。 + * @throws NyARException + */ + public boolean detectMarkerLite(NyARRaster i_image,int i_thresh) throws NyARException + { + detected_square=null; + NyARSquareList l_square_list=this.square_list; + //スクエアコードを探す + square.detectSquare(i_image, i_thresh,l_square_list); + + int number_of_square=l_square_list.getSquareNum(); + //コードは見つかった？ + if(number_of_square<1){ + return false; + } + + //評価基準になるパターンをイメージから切り出す + if(!patt.pickFromRaster(i_image,l_square_list.getSquare(0))){ + //パターンの切り出しに失敗 + return false; + } + //パターンを評価器にセット + if(!this.match_patt.setPatt(patt)){ + //計算に失敗した。 + throw new NyARException(); + } + //コードと比較する + match_patt.evaluate(code); + int square_index=0; + int direction=match_patt.getDirection(); + double confidence=match_patt.getConfidence(); + for(int i=1;ic2){ + continue; + } + //もっと一致するマーカーがあったぽい + square_index=i; + direction=match_patt.getDirection(); + confidence=c2; + } + //マーカー情報を保存 + detected_square=l_square_list.getSquare(square_index); + detected_direction=direction; + detected_confidence=confidence; + return true; + } + /** + * 検出したマーカーの変換行列を計算して、o_resultへ値を返します。 + * 直前に実行したdetectMarkerLiteが成功していないと使えません。 + * @param o_result + * 変換行列を受け取るオブジェクトを指定します。 + * @throws NyARException + */ + public void getTransmationMatrix(NyARTransMatResult o_result) throws NyARException + { + //一番一致したマーカーの位置とかその辺を計算 + if(is_continue){ + transmat.transMatContinue(detected_square,detected_direction,marker_width,o_result); + }else{ + transmat.transMat(detected_square,detected_direction,marker_width,o_result); + } + return; + } + /** + * 検出したマーカーの一致度を返します。 + * @return + * マーカーの一致度を返します。0～1までの値をとります。 + * 一致度が低い場合には、誤認識の可能性が高くなります。 + * @throws NyARException + */ + public double getConfidence() + { + return detected_confidence; + } + /** + * 検出したマーカーの方位を返します。 + * @return + * 0,1,2,3の何れかを返します。 + */ + public int getDirection() + { + return detected_direction; + } + /** + * getTransmationMatrixの計算モードを設定します。 + * 初期値はTRUEです。 + * @param i_is_continue + * TRUEなら、transMatCont互換の計算をします。 + * FALSEなら、transMat互換の計算をします。 + */ + public void setContinueMode(boolean i_is_continue) + { + this.is_continue=i_is_continue; + } +} + + + + + + + + + + + + + + + diff --git a/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARDetectMarker.java b/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARDetectMarker.java new file mode 100644 index 0000000..7425cb8 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARDetectMarker.java @@ -0,0 +1,64 @@ +/** + * NyARSingleDetectMarkerにOpenGL向け関数を追加したもの + * (c)2008 A虎＠nyatla.jp + * airmail(at)ebony.plala.or.jp + * http://nyatla.jp/ + */ +package jp.nyatla.nyartoolkit.jogl.utils; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.NyARCode; +import jp.nyatla.nyartoolkit.core.NyARParam; +import jp.nyatla.nyartoolkit.core.NyARTransMatResult; +import jp.nyatla.nyartoolkit.detector.*; + +public class GLNyARDetectMarker extends NyARDetectMarker +{ + private NyARTransMatResult trans_mat_result=new NyARTransMatResult(); + private double view_scale_factor=0.025;//#define VIEW_SCALEFACTOR 0.025 // 1.0 ARToolKit unit becomes 0.025 of my OpenGL units. + public GLNyARDetectMarker(NyARParam i_param,NyARCode[] i_code,double[] i_marker_width,int i_number_of_code) throws NyARException + { + super(i_param,i_code,i_marker_width,i_number_of_code); + } + public void setScaleFactor(double i_new_value) + { + view_scale_factor=i_new_value; + } + /** + * @param i_index + * マーカーのインデックス番号を指定します。 + * 直前に実行したdetectMarkerLiteの戻り値未満かつ0以上である必要があります。 + * @param o_result + * 結果値を格納する配列を指定してください。double[16]以上が必要です。 + * @throws NyARException + */ + public void getCameraViewRH(int i_index,double[] o_result) throws NyARException + { + //座標を計算 + this.getTransmationMatrix(i_index,this.trans_mat_result); + //行列変換 + double[][] para=this.trans_mat_result.getArray(); + o_result[0 + 0*4] = para[0][0]; // R1C1 + o_result[0 + 1*4] = para[0][1]; // R1C2 + o_result[0 + 2*4] = para[0][2]; + o_result[0 + 3*4] = para[0][3]; + o_result[1 + 0*4] = -para[1][0]; // R2 + o_result[1 + 1*4] = -para[1][1]; + o_result[1 + 2*4] = -para[1][2]; + o_result[1 + 3*4] = -para[1][3]; + o_result[2 + 0*4] = -para[2][0]; // R3 + o_result[2 + 1*4] = -para[2][1]; + o_result[2 + 2*4] = -para[2][2]; + o_result[2 + 3*4] = -para[2][3]; + o_result[3 + 0*4] = 0.0; + o_result[3 + 1*4] = 0.0; + o_result[3 + 2*4] = 0.0; + o_result[3 + 3*4] = 1.0; + if (view_scale_factor != 0.0) { + o_result[12] *= view_scale_factor; + o_result[13] *= view_scale_factor; + o_result[14] *= view_scale_factor; + } + return; + } +} diff --git a/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARParam.java b/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARParam.java new file mode 100644 index 0000000..dddc7b2 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARParam.java @@ -0,0 +1,117 @@ +/** + * NyARParamにOpenGL向け関数を追加したもの + * (c)2008 A虎＠nyatla.jp + * airmail(at)ebony.plala.or.jp + * http://nyatla.jp/ + */ +package jp.nyatla.nyartoolkit.jogl.utils; + +import jp.nyatla.nyartoolkit.core.NyARMat; +import jp.nyatla.nyartoolkit.core.NyARParam; + +public class GLNyARParam extends NyARParam { + private double view_distance_min = 0.1;// #define VIEW_DISTANCE_MIN 0.1 // + // Objects closer to the camera than + // this will not be displayed. + private double view_distance_max = 100.0;// #define VIEW_DISTANCE_MAX + // 100.0 // Objects further away + // from the camera than this + // will not be displayed. + private double[] m_projection = null; + + private float[] m_projectionF = null; + + public void setViewDistanceMin(double i_new_value) { + m_projection = null;// キャッシュ済変数初期化 + view_distance_min = i_new_value; + } + + public void setViewDistanceMax(double i_new_value) { + m_projection = null;// キャッシュ済変数初期化 + view_distance_max = i_new_value; + } + + /** + * void arglCameraFrustumRH(const ARParam *cparam, const double focalmin, + * const double focalmax, GLdouble m_projection[16]) 関数の置き換え + * + * @param focalmin + * @param focalmax + * @return + */ + public double[] getCameraFrustumRH() { + // 既に値がキャッシュされていたらそれを使う + if (m_projection != null) { + return m_projection; + } + // 無ければ計算 + m_projection = new double[16]; + NyARMat trans_mat = new NyARMat(3, 4); + NyARMat icpara_mat = new NyARMat(3, 4); + double[][] p = new double[3][3], q = new double[4][4]; + int width, height; + int i, j; + + width = xsize; + height = ysize; + + decompMat(icpara_mat, trans_mat); + + double[][] icpara = icpara_mat.getArray(); + double[][] trans = trans_mat.getArray(); + for (i = 0; i < 4; i++) { + icpara[1][i] = (height - 1) * (icpara[2][i]) - icpara[1][i]; + } + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + p[i][j] = icpara[i][j] / icpara[2][2]; + } + } + q[0][0] = (2.0 * p[0][0] / (width - 1)); + q[0][1] = (2.0 * p[0][1] / (width - 1)); + q[0][2] = -((2.0 * p[0][2] / (width - 1)) - 1.0); + q[0][3] = 0.0; + + q[1][0] = 0.0; + q[1][1] = -(2.0 * p[1][1] / (height - 1)); + q[1][2] = -((2.0 * p[1][2] / (height - 1)) - 1.0); + q[1][3] = 0.0; + + q[2][0] = 0.0; + q[2][1] = 0.0; + q[2][2] = (view_distance_max + view_distance_min) + / (view_distance_min - view_distance_max); + q[2][3] = 2.0 * view_distance_max * view_distance_min + / (view_distance_min - view_distance_max); + + q[3][0] = 0.0; + q[3][1] = 0.0; + q[3][2] = -1.0; + q[3][3] = 0.0; + + for (i = 0; i < 4; i++) { // Row. + // First 3 columns of the current row. + for (j = 0; j < 3; j++) { // Column. + m_projection[i + j * 4] = q[i][0] * trans[0][j] + q[i][1] + * trans[1][j] + q[i][2] * trans[2][j]; + } + // Fourth column of the current row. + m_projection[i + 3 * 4] = q[i][0] * trans[0][3] + q[i][1] + * trans[1][3] + q[i][2] * trans[2][3] + q[i][3]; + } + return m_projection; + } + + public float[] getCameraFrustumRHf() { + // 既に値がキャッシュされていたらそれを使う + if (m_projectionF == null) { + double[] mf = getCameraFrustumRH(); + m_projectionF = new float[mf.length]; + for (int i = 0; i < mf.length; i++) { + m_projectionF[i] = (float) mf[i]; + } + } + return m_projectionF; + } +} diff --git a/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARSingleDetectMarker.java b/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARSingleDetectMarker.java new file mode 100644 index 0000000..1ad453f --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/jogl/utils/GLNyARSingleDetectMarker.java @@ -0,0 +1,108 @@ +/** + * NyARSingleDetectMarkerにOpenGL向け関数を追加したもの + * (c)2008 A虎＠nyatla.jp + * airmail(at)ebony.plala.or.jp + * http://nyatla.jp/ + */ +package jp.nyatla.nyartoolkit.jogl.utils; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.NyARCode; +import jp.nyatla.nyartoolkit.core.NyARParam; +import jp.nyatla.nyartoolkit.core.NyARTransMatResult; +import jp.nyatla.nyartoolkit.detector.*; + +public class GLNyARSingleDetectMarker extends NyARSingleDetectMarker { + private NyARTransMatResult trans_mat_result = new NyARTransMatResult(); + private double view_scale_factor = 0.025;// #define VIEW_SCALEFACTOR + + // 0.025 // 1.0 ARToolKit unit + // becomes 0.025 of my OpenGL + // units. + + public GLNyARSingleDetectMarker(NyARParam i_param, NyARCode i_code, + double i_marker_width) throws NyARException { + super(i_param, i_code, i_marker_width); + } + + public void setScaleFactor(double i_new_value) { + view_scale_factor = i_new_value; + } + + // public static void arglCameraViewRH(const double para[3][4], GLdouble + // m_modelview[16], const double scale) + public double[] getCameraViewRH() throws NyARException { + double[] result = new double[16]; + getCameraViewRH(result); + return result; + } + + /** + * + * @param o_result + * 結果値を格納する配列を指定してください。double[16]以上が必要です。 + * @throws NyARException + */ + public void getCameraViewRH(double[] o_result) throws NyARException { + // 座標を計算 + this.getTransmationMatrix(this.trans_mat_result); + // 行列変換 + double[][] para = this.trans_mat_result.getArray(); + o_result[0 + 0 * 4] = para[0][0]; // R1C1 + o_result[0 + 1 * 4] = para[0][1]; // R1C2 + o_result[0 + 2 * 4] = para[0][2]; + o_result[0 + 3 * 4] = para[0][3]; + o_result[1 + 0 * 4] = -para[1][0]; // R2 + o_result[1 + 1 * 4] = -para[1][1]; + o_result[1 + 2 * 4] = -para[1][2]; + o_result[1 + 3 * 4] = -para[1][3]; + o_result[2 + 0 * 4] = -para[2][0]; // R3 + o_result[2 + 1 * 4] = -para[2][1]; + o_result[2 + 2 * 4] = -para[2][2]; + o_result[2 + 3 * 4] = -para[2][3]; + o_result[3 + 0 * 4] = 0.0; + o_result[3 + 1 * 4] = 0.0; + o_result[3 + 2 * 4] = 0.0; + o_result[3 + 3 * 4] = 1.0; + if (view_scale_factor != 0.0) { + o_result[12] *= view_scale_factor; + o_result[13] *= view_scale_factor; + o_result[14] *= view_scale_factor; + } + return; + } + + /** + * + * @param o_result + * 結果値を格納する配列を指定してください。double[16]以上が必要です。 + * @throws NyARException + */ + public void getCameraViewRH(float[] o_result) throws NyARException { + // 座標を計算 + this.getTransmationMatrix(this.trans_mat_result); + // 行列変換 + double[][] para = this.trans_mat_result.getArray(); + o_result[0 + 0 * 4] = (float) para[0][0]; // R1C1 + o_result[0 + 1 * 4] = (float) para[0][1]; // R1C2 + o_result[0 + 2 * 4] = (float) para[0][2]; + o_result[0 + 3 * 4] = (float) para[0][3]; + o_result[1 + 0 * 4] = (float) -para[1][0]; // R2 + o_result[1 + 1 * 4] = (float) -para[1][1]; + o_result[1 + 2 * 4] = (float) -para[1][2]; + o_result[1 + 3 * 4] = (float) -para[1][3]; + o_result[2 + 0 * 4] = (float) -para[2][0]; // R3 + o_result[2 + 1 * 4] = (float) -para[2][1]; + o_result[2 + 2 * 4] = (float) -para[2][2]; + o_result[2 + 3 * 4] = (float) -para[2][3]; + o_result[3 + 0 * 4] = (float) 0.0; + o_result[3 + 1 * 4] = (float) 0.0; + o_result[3 + 2 * 4] = (float) 0.0; + o_result[3 + 3 * 4] = (float) 1.0; + if (view_scale_factor != 0.0) { + o_result[12] *= view_scale_factor; + o_result[13] *= view_scale_factor; + o_result[14] *= view_scale_factor; + } + } +} diff --git a/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java b/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java new file mode 100644 index 0000000..8fc2013 --- /dev/null +++ b/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java @@ -0,0 +1,97 @@ +/* + * PROJECT: NyARToolkit + * -------------------------------------------------------------------------------- + * + * The NyARToolkit is Java version ARToolkit class library. + * Copyright (C)2008 R.Iizuka + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this framework; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * For further information please contact. + * http://nyatla.jp/nyatoolkit/ + * + * + */ +package jp.nyatla.nyartoolkit.sample; + + +import java.io.*; +import java.util.*; + +import jp.nyatla.nyartoolkit.core.*; +import jp.nyatla.nyartoolkit.core.raster.*; +import jp.nyatla.nyartoolkit.detector.*; + +/** + * 320x240のBGRA32で記録されたRAWイメージから、１種類のパターンを認識し、 + * その変換行列を1000回求め、それにかかったミリ秒時間を表示します。 + * + */ +public class RawFileTest { + private final String code_file ="../Data/patt.hiro"; + private final String data_file ="../Data/320x240ABGR.raw"; + private final String camera_file="../Data/camera_para.dat"; + public RawFileTest() + { + } + public void Test_arDetectMarkerLite() throws Exception + { + //AR用カメラパラメタファイルをロード + NyARParam ap =new NyARParam(); + ap.loadFromARFile(camera_file); + ap.changeSize(320,240); + + //AR用のパターンコードを読み出し + NyARCode code=new NyARCode(16,16); + code.loadFromARFile(code_file); + + //試験イメージの読み出し(320x240 BGRAのRAWデータ) + File f=new File(data_file); + FileInputStream fs=new FileInputStream(data_file); + byte[] buf=new byte[(int)f.length()]; + fs.read(buf); + NyARRaster_BGRA ra=NyARRaster_BGRA.wrap(buf, 320, 240); + // Blank_Raster ra=new Blank_Raster(320, 240); + + //１パターンのみを追跡するクラスを作成 + NyARSingleDetectMarker ar=new NyARSingleDetectMarker(ap,code,80.0); + NyARTransMatResult result_mat=new NyARTransMatResult(); + ar.setContinueMode(false); + ar.detectMarkerLite(ra,100); + ar.getTransmationMatrix(result_mat); + + //マーカーを検出 + Date d2=new Date(); + for(int i=0;i<1000;i++){ + //変換行列を取得 + ar.detectMarkerLite(ra,100); + ar.getTransmationMatrix(result_mat); + } + Date d=new Date(); + System.out.println(d.getTime()-d2.getTime()); + } + public static void main(String[] args) + { + + try{ + RawFileTest t=new RawFileTest(); + //t.Test_arGetVersion(); + t.Test_arDetectMarkerLite(); + }catch(Exception e){ + e.printStackTrace(); + } + } + +} diff --git a/src/jp/nyatla/util/BytePointer.java b/src/jp/nyatla/util/BytePointer.java new file mode 100644 index 0000000..1de1df9 --- /dev/null +++ b/src/jp/nyatla/util/BytePointer.java @@ -0,0 +1,56 @@ +package jp.nyatla.util; + +public class BytePointer { + private byte[] array_ref; //配列 + private int array_offset; //配列に対する基準値 + private int position; //array_offsetに対する現在位置 + public static BytePointer wrap(byte[] i_array_ref,int i_offset) + { + return new BytePointer(i_array_ref,i_offset); + } + public void set(byte i_value) + { + array_ref[array_offset+position]=i_value; + } + public void set(int i_rel_positon,byte i_value) + { + array_ref[array_offset+position+i_rel_positon]=i_value; + } + /** + * カレント位置の値を取得する + * @return + */ + public byte get() + { + return array_ref[array_offset+position]; + } + /** + * カレント位置から+i_slideの位置にある値を取得する。 + * @param i_step + * @return + */ + public byte get(int i_slide) + { + return array_ref[array_offset+position+i_slide]; + } + public void incPtr() + { + position++; + } + public void addPtr(int v) + { + position+=v; + } + private BytePointer(byte[] i_array_ref,int i_base_point) + { + array_offset =i_base_point; + array_ref =i_array_ref; + position =0; + } +// public BytePointer() +// { +// array_offset =0; +// array_ref =new int[1]; +// position =0; +// } +} diff --git a/src/jp/nyatla/util/DoublePointer.java b/src/jp/nyatla/util/DoublePointer.java new file mode 100644 index 0000000..28abf0e --- /dev/null +++ b/src/jp/nyatla/util/DoublePointer.java @@ -0,0 +1,105 @@ +package jp.nyatla.util; +/** + * double型ポインタのエミュレートクラス + * 対象のdouble配列を基点を基準に参照する。 + * + */ +public class DoublePointer +{ + private double[] array_ref; //配列 + private int array_offset; //配列に対する基準値 + private int position; //array_offsetに対する現在位置 + public static DoublePointer wrap(double[] i_array_ref,int i_offset) + { + return new DoublePointer(i_array_ref,i_offset); + } + public static DoublePointer wrap(DoublePointer i_inst) + { + return new DoublePointer(i_inst.array_ref,i_inst.getPtrArrayOffset()); + } + //現在位置からのサブシーケンスを返す。 + public DoublePointer slice(int i_offset) + { + return DoublePointer.wrap(array_ref,array_offset+position+i_offset); + } + public void set(double i_value) + { + array_ref[array_offset+position]=i_value; + } + public void set(int i_rel_positon,double i_value) + { + array_ref[array_offset+position+i_rel_positon]=i_value; + } + /** + * カレント位置の値を取得する + * @return + */ + public double get() + { + return array_ref[array_offset+position]; + } + /** + * カレント位置から+i_slideの位置にある値を取得する。 + * @param i_step + * @return + */ + public double get(int i_slide) + { + return array_ref[array_offset+position+i_slide]; + } + public void incPtr() + { + position++; + } + public void addPtr(int v) + { + position+=v; + } + public double[] array() + { + return array_ref; + } + public void subValue(double i_val) + { + array_ref[array_offset+position]-=i_val; + } + public void subValue(int i_step,double i_val) + { + array_ref[array_offset+position+i_step]-=i_val; + } + public void addValue(double i_val) + { + array_ref[array_offset+position]+=i_val; + } + public void addValue(int i_step,double i_val) + { + array_ref[array_offset+position+i_step]+=i_val; + } + + /** + * 現在位置のオフセット位置を返す。 + * @return + */ + public int getPtrArrayOffset() + { + return array_offset+position; + } + private DoublePointer(double[] i_array_ref,int i_base_point) + { + array_offset =i_base_point; + array_ref =i_array_ref; + position =0; + } + public DoublePointer(int i_length) + { + array_offset =0; + array_ref =new double[i_length]; + position =0; + } + public DoublePointer() + { + array_offset =0; + array_ref =new double[1]; + position =0; + } +} \ No newline at end of file diff --git a/src/jp/nyatla/util/DoubleValue.java b/src/jp/nyatla/util/DoubleValue.java new file mode 100644 index 0000000..a42bd14 --- /dev/null +++ b/src/jp/nyatla/util/DoubleValue.java @@ -0,0 +1,5 @@ +package jp.nyatla.util; + +public class DoubleValue { + public double value; +} \ No newline at end of file diff --git a/src/jp/nyatla/util/IntPointer.java b/src/jp/nyatla/util/IntPointer.java new file mode 100644 index 0000000..e655913 --- /dev/null +++ b/src/jp/nyatla/util/IntPointer.java @@ -0,0 +1,64 @@ +package jp.nyatla.util; + +public class IntPointer { + private int[] array_ref; //配列 + private int array_offset; //配列に対する基準値 + private int position; //array_offsetに対する現在位置 + public static IntPointer wrap(int[] i_array_ref,int i_offset) + { + return new IntPointer(i_array_ref,i_offset); + } + public void set(int i_value) + { + array_ref[array_offset+position]=i_value; + } + public void set(int i_rel_positon,int i_value) + { + array_ref[array_offset+position+i_rel_positon]=i_value; + } + /** + * カレント位置の値を取得する + * @return + */ + public int get() + { + return array_ref[array_offset+position]; + } + /** + * カレント位置から+i_slideの位置にある値を取得する。 + * @param i_step + * @return + */ + public int get(int i_slide) + { + return array_ref[array_offset+position+i_slide]; + } + public void incPtr() + { + position++; + } + public void addPtr(int v) + { + position+=v; + } + public void addValue(int i_val) + { + array_ref[array_offset+position]+=i_val; + } + public void addValue(int i_step,int i_val) + { + array_ref[array_offset+position+i_step]+=i_val; + } + private IntPointer(int[] i_array_ref,int i_base_point) + { + array_offset =i_base_point; + array_ref =i_array_ref; + position =0; + } + public IntPointer() + { + array_offset =0; + array_ref =new int[1]; + position =0; + } +} diff --git a/src/jp/nyatla/util/IntValue.java b/src/jp/nyatla/util/IntValue.java new file mode 100644 index 0000000..be2d7da --- /dev/null +++ b/src/jp/nyatla/util/IntValue.java @@ -0,0 +1,17 @@ +package jp.nyatla.util; + +public class IntValue { + public int value; +/* public void set(int i_v){ + v=i_v; + } + public int get(){ + return v; + } + public void inc(){ + v++; + } + public void add(int i_v){ + v+=i_v; + }*/ +} diff --git a/src/jp/nyatla/util/ShortPointer.java b/src/jp/nyatla/util/ShortPointer.java new file mode 100644 index 0000000..7c75a1a --- /dev/null +++ b/src/jp/nyatla/util/ShortPointer.java @@ -0,0 +1,60 @@ +package jp.nyatla.util; + +public class ShortPointer { + private short[] array_ref; //配列 + private int array_offset; //配列に対する基準値 + private int position; //array_offsetに対する現在位置 + public static ShortPointer wrap(short[] i_array_ref,int i_offset) + { + return new ShortPointer(i_array_ref,i_offset); + } + public static ShortPointer wrap(ShortPointer i_inst,int i_offset) + { + return new ShortPointer(i_inst.array_ref,i_inst.array_offset+i_inst.position+i_offset); + } + public void set(short i_value) + { + array_ref[array_offset+position]=i_value; + } + public void set(int i_rel_positon,short i_value) + { + array_ref[array_offset+position+i_rel_positon]=i_value; + } + /** + * カレント位置の値を取得する + * @return + */ + public short get() + { + return array_ref[array_offset+position]; + } + /** + * カレント位置から+i_slideの位置にある値を取得する。 + * @param i_step + * @return + */ + public short get(int i_slide) + { + return array_ref[array_offset+position+i_slide]; + } + public void incPtr() + { + position++; + } + public void addPtr(int v) + { + position+=v; + } + private ShortPointer(short[] i_array_ref,int i_base_point) + { + array_offset =i_base_point; + array_ref =i_array_ref; + position =0; + } +// public BytePointer() +// { +// array_offset =0; +// array_ref =new int[1]; +// position =0; +// } +} diff --git a/src/jp/nyatla/util/StringPointer.java b/src/jp/nyatla/util/StringPointer.java new file mode 100644 index 0000000..dc2244b --- /dev/null +++ b/src/jp/nyatla/util/StringPointer.java @@ -0,0 +1,25 @@ +package jp.nyatla.util; + +public class StringPointer { + private String[] array_ref;//配列 + private int array_offset; //配列に対する基準値 + private int position; //array_offsetに対する現在位置 + public void set(String i_value) + { + array_ref[array_offset+position]=i_value; + } + public String get() + { + return array_ref[array_offset+position]; + } + public String toString() + { + return get(); + } + public StringPointer() + { + array_ref=new String[1]; + array_offset=0; + position=0; + } +} diff --git a/src/jp/nyatla/util/StringValue.java b/src/jp/nyatla/util/StringValue.java new file mode 100644 index 0000000..47102c6 --- /dev/null +++ b/src/jp/nyatla/util/StringValue.java @@ -0,0 +1,25 @@ +package jp.nyatla.util; + +public class StringValue { + private String v; + public StringValue() + { + v=""; + } + public StringValue(String i_v) + { + v=i_v; + } + public void set(String i_v) + { + v=i_v; + } + public String get() + { + return v; + } + public String toString() + { + return v; + } +}