tags/2.3.1/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyMath.java

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  * \r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.sandbox.x2;\r
  33 \r
  34 public class NyMath\r
  35 {\r
  36         public final static long FIXEDFLOAT24_1=0x1000000L;\r
  37         public final static long FIXEDFLOAT24_0_25=FIXEDFLOAT24_1/4;\r
  38         public final static long FIXEDFLOAT16_1=0x10000L;       \r
  39         public final static long FIXEDFLOAT16_0_25=FIXEDFLOAT16_1/4;    \r
  40         public final static long FIXEDFLOAT8_1=0x100L;\r
  41         \r
  42         private final static int FIXEDFLOAT16I_1=(int)FIXEDFLOAT16_1;   \r
  43         private final static int FIXEDFLOAT16I_0_25=(int)FIXEDFLOAT16_1/4;      \r
  44         \r
  45         \r
  46         \r
  47 \r
  48         private final static int FF16_PI=(int)(Math.PI*FIXEDFLOAT16_1);\r
  49         private final static int FF16_2PI=(int)(2 *FF16_PI);\r
  50         private final static int FF16_05PI=(int)(FF16_PI/2);\r
  51         /* sinテーブルは0-2PIを1024分割\r
  52          * acosテーブルは0-1を256分割\r
  53          */\r
  54         private final static int[] sin_table=new int[339];\r
  55         private final static int[] acos_table=new int[1537];\r
  56         private final static int SQRT_LOOP=10;\r
  57         /**\r
  58          * http://www.geocities.co.jp/SiliconValley-PaloAlto/5438/\r
  59          * 参考にしました。\r
  60          * 少数点部が16bitの変数の平方根を求めます。\r
  61          * 戻り値の小数点部分は16bitです。\r
  62          * @param i_v\r
  63          * @return\r
  64          */\r
  65         public static long sqrtFixdFloat16(long i_ff16)\r
  66         {\r
  67                 long t=0,s;\r
  68                 s=i_ff16>0?i_ff16:-i_ff16;\r
  69                 if(i_ff16==0){\r
  70                         return 0;\r
  71                 }\r
  72                 for(int i=SQRT_LOOP;i>0;i--){\r
  73                         t = s;\r
  74                         s = (t+((i_ff16<<16)/t))>>1;\r
  75                         if(s==t){\r
  76                                 break;\r
  77                         }\r
  78                 };\r
  79                 return t;\r
  80         }\r
  81         public static long sqrtFixdFloat(long i_ff,int i_bit)\r
  82         {\r
  83                 long t=0,s;\r
  84                 s=i_ff>0?i_ff:-i_ff;\r
  85                 if(i_ff==0){\r
  86                         return 0;\r
  87                 }\r
  88                 for(int i=SQRT_LOOP;i>0;i--){\r
  89                         t = s;\r
  90                         s = (t+((i_ff<<i_bit)/t))>>1;\r
  91                         if(s==t){\r
  92                                 break;\r
  93                         }\r
  94                 }\r
  95                 return t;\r
  96         }\r
  97         public static int acosFixedFloat16(int i_ff24)\r
  98         {/*     \r
  99                 long x=i_ff24>>8;\r
 100                 long x2=(x*x)>>16;\r
 101                 long x3=(x2*x)>>16;\r
 102                 long x4=(x2*x2)>>16;\r
 103 //              return FF16_05PI-(int)(x+x3/6+(((3*x3*x2/(2*4*5)+(3*5*x4*x3)/(2*4*6*7)))>>16));\r
 104 */              \r
 105                 int result;\r
 106                 int abs_ff24=i_ff24>0?i_ff24:-i_ff24;\r
 107                 //(0<=n<=0.25) 0<=0-16384(65536/4)まではy=PI/2-xので近似\r
 108                 //(0.25<n<=1/2PI) 16385-65536までは(128ステップ単位)のテーブルを使用                           \r
 109 \r
 110                 if(abs_ff24<FIXEDFLOAT24_0_25){\r
 111                         //0.25までの範囲は、2次の近似式\r
 112                         result=(i_ff24>>8);\r
 113                         return FF16_05PI-result+((((result*result)>>16)*result)>>16)/6;\r
 114                 }else{\r
 115                         result=acos_table[((abs_ff24>>8)-FIXEDFLOAT16I_0_25)>>5];\r
 116                         if (i_ff24 < 0) {\r
 117                                 return FF16_PI-result;\r
 118                         }else{\r
 119                                 return result;\r
 120                         }\r
 121                 }\r
 122 //              return (int)(Math.acos((double)i_ff24/0x1000000)*0x10000);\r
 123         }\r
 124         /**\r
 125          * 誤差確認用の関数\r
 126          * @param args\r
 127          */\r
 128         public static void main(String[] args)\r
 129         {\r
 130                 //sin\r
 131                 NyMath.initialize();\r
 132 //              for(int i=0;i<3600;i++){\r
 133 //                      int s=cosFixedFloat24((int)(FIXEDFLOAT16I_1*i*Math.PI/1800));\r
 134 //                      System.out.println((double)(s-(int)(FIXEDFLOAT24_1*Math.cos((i*Math.PI/1800))))/FIXEDFLOAT24_1);\r
 135 //              }\r
 136                 //acos\r
 137                 for(int i=-1000;i<1000;i++){\r
 138                         int s=acosFixedFloat16((int)(FIXEDFLOAT24_1*i/1000));\r
 139 //                      System.out.println((double)(s-(int)(FIXEDFLOAT16_1*Math.acos((double)i/1000)))/FIXEDFLOAT16_1);\r
 140                         System.out.println((double)s/FIXEDFLOAT16I_1);\r
 141                 }\r
 142         }\r
 143         public static int sinFixedFloat24(int i_ff16)\r
 144         {\r
 145                 int result;\r
 146                 //i_ff16を0-2πに制限\r
 147                 int rad=i_ff16%FF16_2PI;\r
 148                 if(rad<0){\r
 149                         rad=rad+FF16_2PI;\r
 150                 }\r
 151                 //4ブロックに分割\r
 152                 int dv=rad/FF16_05PI;\r
 153                 //radを0-0.5PIに制限\r
 154                 rad=rad-dv*FF16_05PI;\r
 155                 //radをdvにより補正\r
 156                 if(dv==1 || dv==3){\r
 157                         rad=FF16_05PI-rad;\r
 158                 }\r
 159                 //(0<=n<=0.25) 0<=0-16384(65536/4)まではy=xので近似\r
 160                 //(0.25<n<=1/2PI) 16385-102944までは(256ステップ単位)のテーブルを使用                          \r
 161                 //負にする\r
 162                 if(rad<FIXEDFLOAT16_0_25){\r
 163                         result=rad<<8;\r
 164                 }else{\r
 165                         result=sin_table[(rad-FIXEDFLOAT16I_0_25)>>8];\r
 166                 }\r
 167                 if(dv>=2){\r
 168                         result=-result;\r
 169                 }\r
 170                 return result;\r
 171 //              return (int)(Math.sin((double)i_ff16/0x10000)*0x1000000);\r
 172         }\r
 173         public static int cosFixedFloat24(int i_ff16)\r
 174         {\r
 175                 int result;\r
 176                 //i_ff16を0-2πに制限\r
 177                 int rad=(i_ff16+FF16_05PI)%FF16_2PI;\r
 178                 if(rad<0){\r
 179                         rad=rad+FF16_2PI;\r
 180                 }\r
 181                 //4ブロックに分割\r
 182                 int dv=rad/FF16_05PI;\r
 183                 //radを0-0.5PIに制限\r
 184                 rad=rad-dv*FF16_05PI;\r
 185                 //radをdvにより補正\r
 186                 if(dv==1 || dv==3){\r
 187                         rad=FF16_05PI-rad;\r
 188                 }\r
 189                 //(0<=n<=0.25) 0<=0-16384(65536/4)まではy=xので近似\r
 190                 //(0.25<n<=1/2PI) 16385-102944までは(256ステップ単位)のテーブルを使用                          \r
 191                 //負にする\r
 192                 if(rad<FIXEDFLOAT16_0_25){\r
 193                         result=rad<<8;\r
 194                 }else{\r
 195                         result=sin_table[(rad-FIXEDFLOAT16I_0_25)>>8];\r
 196                 }\r
 197                 if(dv>=2){\r
 198                         result=-result;\r
 199                 }\r
 200                 return result;\r
 201 //              return (int)(Math.cos((double)i_ff16/0x10000)*0x1000000);\r
 202         }\r
 203         public static void initialize()\r
 204         {\r
 205 \r
 206                 int step;\r
 207                 step=FIXEDFLOAT16I_0_25+256;\r
 208                 for(int i=0;i<339;i++){\r
 209                         sin_table[i]=(int)((Math.sin((double) step / (double) FIXEDFLOAT16I_1))*FIXEDFLOAT24_1);\r
 210                         step+=256;\r
 211                 }\r
 212                 //acosテーブル初期化\r
 213                 step=FIXEDFLOAT16I_0_25+32;\r
 214                 for (int i = 0; i < 1537; i++) {\r
 215                         acos_table[i] =(int)((Math.acos((double) step/(double) FIXEDFLOAT16I_1))*FIXEDFLOAT16_1);\r
 216                         step+=32;\r
 217                 }\r
 218                 return;\r
 219         }\r
 220         public static void printF16(long i_value)\r
 221         {\r
 222                 System.out.println((double)i_value/0x10000);\r
 223                 return;\r
 224         }\r
 225         public static void printF24(long i_value)\r
 226         {\r
 227                 System.out.println((double)i_value/0x1000000);\r
 228                 return;\r
 229         }\r
 230 }\r