natives.c
Go to the documentation of this file.
00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2007--2011:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006                 and other contributors, see authors.txt for details
00007         
00008         This program is free software: you can redistribute it and/or modify
00009         it under the terms of the GNU Lesser General Public License as published
00010         by the Free Software Foundation, version 3 of the License.
00011         
00012         This program is distributed in the hope that it will be useful,
00013         but WITHOUT ANY WARRANTY; without even the implied warranty of
00014         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015         GNU Lesser General Public License for more details.
00016         
00017         You should have received a copy of the GNU Lesser General Public License
00018         along with this program. If not, see <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include "../common/consts.h"
00022 #include "../common/types.h"
00023 #include "natives.h"
00024 #include <string.h>
00025 
00026 #include <assert.h>
00027 
00028 #if defined(__dsPIC30F__)
00029 #include <p30fxxxx.h>
00030 #define DSP_AVAILABLE
00031 #elif defined(__dsPIC33F__)
00032 #include <p33Fxxxx.h>
00033 #define DSP_AVAILABLE
00034 #elif defined(__PIC24H__)
00035 #include <p24Hxxxx.h>
00036 #endif 
00037 
00038 
00039 
00047 
00048 // useful math functions used by below
00049 
00050 // table is 20 bins (one for each bit of value) of 8 values each + one for infinity
00051 static const sint16 aseba_atan_table[20*8+1] = { 652, 735, 816, 896, 977, 1058, 1139, 1218, 1300, 1459, 1620, 1777, 1935, 2093, 2250, 2403, 2556, 2868, 3164, 3458, 3748, 4029, 4307, 4578, 4839, 5359, 5836, 6290, 6720, 7126, 7507, 7861, 8203, 8825, 9357, 9839, 10260, 10640, 10976, 11281, 11557, 12037, 12425, 12755, 13036, 13277, 13486, 13671, 13837, 14112, 14331, 14514, 14666, 14796, 14907, 15003, 15091, 15235, 15348, 15441, 15519, 15585, 15642, 15691, 15736, 15808, 15865, 15912, 15951, 15984, 16013, 16037, 16060, 16096, 16125, 16148, 16168, 16184, 16199, 16211, 16222, 16240, 16255, 16266, 16276, 16284, 16292, 16298, 16303, 16312, 16320, 16325, 16331, 16334, 16338, 16341, 16344, 16348, 16352, 16355, 16357, 16360, 16361, 16363, 16364, 16366, 16369, 16369, 16371, 16372, 16373, 16373, 16375, 16375, 16377, 16376, 16378, 16378, 16378, 16379, 16379, 16380, 16380, 16380, 16382, 16381, 16381, 16381, 16382, 16382, 16382, 16382, 16382, 16382, 16384, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384 };
00052 /* Generation code:
00053 for (int i = 0; i < 20; i++)
00054 {
00055         double d = pow(2, (double)(i - 4));
00056         double dd = d / 8;
00057         for (int j = 0; j < 8; j++)
00058         {
00059                 double v = atan(d + dd * j);
00060                 aseba_atan_table[i*8+j] = (int)(((32768.*v) /  (M_PI))+0.5);
00061         }
00062         aseba_atan_table[20*8] = 16384
00063 }
00064 + optimisation
00065 */
00066 
00067 
00068 // atan2, do y/x and return an "aseba" angle that spans the whole 16 bits range
00069 sint16 aseba_atan2(sint16 y, sint16 x)
00070 {
00071         if (y == 0)
00072         {
00073                 if (x >= 0)     // we return 0 on division by zero
00074                         return 0;
00075                 else if (x < 0)
00076                         return -32768;
00077         }
00078         
00079         {
00080                 sint16 res;
00081                 sint16 ax = abs(x);
00082                 sint16 ay = abs(y);
00083                 if (x == 0)
00084                 {
00085                         res = 16384;
00086                 }
00087                 else
00088                 {
00089 #ifdef __C30__
00090                         unsigned int q2, rem1;
00091                         unsigned long q1;
00092                         q1 = __builtin_divmodud(ay, ax, &rem1);
00093                         q2 = __builtin_divud(((unsigned long) rem1) << 16, ax);
00094                         sint32 value = (q1 << 16) | q2;
00095                         
00096                         sint16 fb1;
00097                         
00098                         // find first bit at one
00099                         // ASM optimisation for 16bits PIC
00100                         // Find first bit from left (MSB) on 32 bits word
00101                         asm ("ff1l %[word], %[b]" : [b] "=r" (fb1) : [word] "r" ((int) (value >> 16)) : "cc");
00102                         if(fb1) 
00103                                 fb1 = 17 - fb1 + 16 - 1; // Bit 0 is "16", fbl = 0 mean no 1 found for ff1l
00104                         else {
00105                                 asm ("ff1l %[word], %[b]" : [b] "=r" (fb1) : [word] "r" ((int) value) : "cc");
00106                                 if(fb1)
00107                                         fb1 = 17 - fb1 - 1; // see above
00108                         }
00109                         {
00110                                 // we only keep 4 bits of precision below comma as atan(x) is like x near 0
00111                                 sint16 index = fb1 - 12;
00112                                 if (index < 0)
00113                                 {
00114                                         // value is smaller than 2e-4
00115                                         res = (value*aseba_atan_table[0]) >> 12;
00116                                 }
00117                                 else
00118                                 {
00119                                         sint32 subprecision_rest = value - (1L << fb1);
00120                                         sint16 to_shift = fb1 - 8; // fb1 >= 12 otherwise index would have been < 0
00121                                         sint16 subprecision_index = (sint16)(subprecision_rest >> to_shift);
00122                                         sint16 bin = subprecision_index >> 5;
00123                                         sint16 delta = subprecision_index & 0x1f;
00124                                         res = __builtin_divsd(__builtin_mulss(aseba_atan_table[index*8 + bin], 32 - delta) + __builtin_mulss(aseba_atan_table[index*8 + bin + 1], delta),32);
00125                                 }
00126                                 
00127 #else           
00128                         sint32 value = (((sint32)ay << 16)/(sint32)(ax));
00129                         sint16 fb1 = 0;
00130                         
00131                         sint16 fb1_counter;
00132                         for (fb1_counter = 0; fb1_counter < 32; fb1_counter++)
00133                                 if ((value >> (sint32)fb1_counter) != 0)
00134                                         fb1 = fb1_counter;
00135                                                 
00136                         {
00137                                 // we only keep 4 bits of precision below comma as atan(x) is like x near 0
00138                                 sint16 index = fb1 - 12;
00139                                 if (index < 0)
00140                                 {
00141                                         // value is smaller than 2e-4
00142                                         res = (sint16)(((sint32)aseba_atan_table[0] * value) >> 12);
00143                                 }
00144                                 else
00145                                 {
00146                                         sint32 subprecision_rest = value - (((sint32) 1) << fb1);
00147                                         sint16 to_shift = fb1 - 8; // fb1 >= 12 otherwise index would have been < 0
00148                                         sint16 subprecision_index = (sint16)(subprecision_rest >> to_shift);
00149                                         sint16 bin = subprecision_index >> 5;
00150                                         sint16 delta = subprecision_index & 0x1f;
00151                                         res = (sint16)(((sint32)aseba_atan_table[index*8 + bin] * (sint32)(32 - delta) + (sint32)aseba_atan_table[index*8 + bin + 1] * (sint32)delta) >> 5);
00152                                 }
00153 #endif
00154                                 // do pi - value if x negative
00155                                 if (x < 0)
00156                                         res = 32768 - res;
00157                         }
00158                 }
00159                 
00160                 if (y > 0)
00161                         return res;
00162                 else
00163                         return -res;
00164         }
00165 }
00166 
00167 // 2 << 7 entries + 1, from 0 to 16384, being from 0 to PI
00168 static const sint16 aseba_sin_table[128+1] = {0, 403, 804, 1207, 1608, 2010, 2411, 2812, 3212, 3612, 4011, 4411, 4808, 5206, 5603, 5998, 6393, 6787, 7180, 7572, 7962, 8352, 8740, 9127, 9513, 9896, 10279, 10660, 11040, 11417, 11794, 12167, 12540, 12911, 13279, 13646, 14010, 14373, 14733, 15091, 15447, 15801, 16151, 16500, 16846, 17190, 17531, 17869, 18205, 18538, 18868, 19196, 19520, 19842, 20160, 20476, 20788, 21097, 21403, 21706, 22006, 22302, 22595, 22884, 23171, 23453, 23732, 24008, 24279, 24548, 24812, 25073, 25330, 25583, 25833, 26078, 26320, 26557, 26791, 27020, 27246, 27467, 27684, 27897, 28106, 28311, 28511, 28707, 28899, 29086, 29269, 29448, 29622, 29792, 29957, 30117, 30274, 30425, 30572, 30715, 30852, 30985, 31114, 31238, 31357, 31471, 31581, 31686, 31786, 31881, 31972, 32057, 32138, 32215, 32285, 32352, 32413, 32470, 32521, 32569, 32610, 32647, 32679, 32706, 32728, 32746, 32758, 32766, 32767, };
00169 /* Generation code:
00170 int i;
00171 for (i = 0; i <= 128; i++)
00172 {
00173         sinTable[i] = (sint16)(32767. * sin( ((M_PI/2.)*(double)i)/128.) + 0.5);
00174 }
00175 + optimisation
00176 */
00177 
00178 
00179 // do the sinus of an "aseba" angle that spans the whole 16 bits range, and return a 1.15 fixed point value
00180 sint16 aseba_sin(sint16 angle)
00181 {
00182         sint16 index;
00183         sint16 subIndex;
00184         sint16 invert;
00185         sint16 lookupAngle;
00186         if (angle < 0)
00187         {
00188                 if (angle < -16384)
00189                         lookupAngle = 32768 + angle;
00190                 else if (angle > -16384)
00191                         lookupAngle = -angle;
00192                 else
00193                         return -32767;
00194                 invert = 1;
00195         }
00196         else
00197         {
00198                 if (angle > 16384)
00199                         lookupAngle = 32767 - angle + 1;
00200                 else if (angle < 16484)
00201                         lookupAngle = angle;
00202                 else
00203                         return 32767;
00204                 invert = 0;
00205         }
00206         
00207         index = lookupAngle >> 7;
00208         subIndex = lookupAngle & 0x7f;
00209         
00210         {
00211                 sint16 result = (sint16)(((sint32)aseba_sin_table[index] * (sint32)(128-subIndex) + (sint32)aseba_sin_table[index+1] * (sint32)(subIndex)) >> 7);
00212                 
00213                 if (invert)
00214                         return -result;
00215                 else
00216                         return result;
00217         }
00218 }
00219 
00220 // do the cos of an "aseba" angle that spans the whole 16 bits range, and return a 1.15 fixed point value
00221 sint16 aseba_cos(sint16 angle)
00222 {
00223         return aseba_sin(16384 + angle);
00224 }
00225 
00226 // Do integer square root ( from Wikipedia )
00227 sint16 aseba_sqrt(sint16 num)
00228 {
00229         sint16 op = num;
00230         sint16 res = 0;
00231         sint16 one = 1 << 14;
00232         
00233         while(one > op)
00234                 one >>= 2;
00235                 
00236         while(one != 0) 
00237         {
00238                 if (op >= res + one) 
00239                 {
00240                         op -= res + one;
00241                         res = (res >> 1) + one;
00242                 }
00243                 else
00244                 {
00245                         res >>= 1;
00246                 }
00247                 one >>= 2;
00248         }
00249         return res;
00250 }
00251 
00252 // comb sort ( from Wikipedia )
00253 void aseba_comb_sort(sint16* input, uint16 size)
00254 {
00255         uint16 gap = size;
00256         uint16 swapped = 0;
00257         uint16 i;
00258 
00259         while ((gap > 1) || swapped)
00260         {
00261                 if (gap > 1)
00262                 {
00263 #ifdef __C30__
00264                         gap = __builtin_divud(__builtin_muluu(gap,4),5);
00265 #else
00266                         gap = (uint16)(((uint32)gap * 4) / 5);
00267 #endif
00268                 }
00269 
00270                 swapped = 0;
00271 
00272                 for (i = 0; gap + i < size; i++)
00273                 {
00274                         if (input[i] - input[i + gap] > 0)
00275                         {
00276                                 sint16 swap = input[i];
00277                                 input[i] = input[i + gap];
00278                                 input[i + gap] = swap;
00279                                 swapped = 1;
00280                         }
00281                 }
00282         }
00283 }
00284 
00285 
00286 // standard natives functions
00287 
00288 void AsebaNative_veccopy(AsebaVMState *vm)
00289 {
00290         // variable pos
00291         uint16 dest = AsebaNativePopArg(vm);
00292         uint16 src = AsebaNativePopArg(vm);
00293         
00294         // variable size
00295         uint16 length = AsebaNativePopArg(vm);
00296         
00297         uint16 i;
00298         
00299         for (i = 0; i < length; i++)
00300         {
00301                 vm->variables[dest++] = vm->variables[src++];
00302         }
00303 }
00304 
00305 const AsebaNativeFunctionDescription AsebaNativeDescription_veccopy =
00306 {
00307         "math.copy",
00308         "copies src to dest element by element",
00309         {
00310                 { -1, "dest" },
00311                 { -1, "src" },
00312                 { 0, 0 }
00313         }
00314 };
00315 
00316 
00317 void AsebaNative_vecfill(AsebaVMState *vm)
00318 {
00319         // variable pos
00320         uint16 dest = AsebaNativePopArg(vm);
00321         uint16 value = AsebaNativePopArg(vm);
00322         
00323         // variable size
00324         uint16 length = AsebaNativePopArg(vm);
00325         
00326         uint16 i;
00327         
00328         for (i = 0; i < length; i++)
00329         {
00330                 vm->variables[dest++] = vm->variables[value];
00331         }
00332 }
00333 
00334 const AsebaNativeFunctionDescription AsebaNativeDescription_vecfill =
00335 {
00336         "math.fill",
00337         "fills dest with constant value",
00338         {
00339                 { -1, "dest" },
00340                 { 1, "value" },
00341                 { 0, 0 }
00342         }
00343 };
00344 
00345 
00346 void AsebaNative_vecaddscalar(AsebaVMState *vm)
00347 {
00348         // variable pos
00349         uint16 dest = AsebaNativePopArg(vm);
00350         uint16 src = AsebaNativePopArg(vm);
00351         uint16 scalar = AsebaNativePopArg(vm);
00352         
00353         // variable size
00354         uint16 length = AsebaNativePopArg(vm);
00355         
00356         const sint16 scalarValue = vm->variables[scalar];
00357         uint16 i;
00358         for (i = 0; i < length; i++)
00359         {
00360                 vm->variables[dest++] = vm->variables[src++] + scalarValue;
00361         }
00362 }
00363 
00364 const AsebaNativeFunctionDescription AsebaNativeDescription_vecaddscalar =
00365 {
00366         "math.addscalar",
00367         "add scalar to each element of src and store result to dest",
00368         {
00369                 { -1, "dest" },
00370                 { -1, "src" },
00371                 { 1, "scalar" },
00372                 { 0, 0 }
00373         }
00374 };
00375 
00376 
00377 void AsebaNative_vecadd(AsebaVMState *vm)
00378 {
00379         // variable pos
00380         uint16 dest = AsebaNativePopArg(vm);
00381         uint16 src1 = AsebaNativePopArg(vm);
00382         uint16 src2 = AsebaNativePopArg(vm);
00383         
00384         // variable size
00385         uint16 length = AsebaNativePopArg(vm);
00386         
00387         uint16 i;
00388         for (i = 0; i < length; i++)
00389         {
00390                 vm->variables[dest++] = vm->variables[src1++] + vm->variables[src2++];
00391         }
00392 }
00393 
00394 const AsebaNativeFunctionDescription AsebaNativeDescription_vecadd =
00395 {
00396         "math.add",
00397         "adds src1 and src2 to dest, element by element",
00398         {
00399                 { -1, "dest" },
00400                 { -1, "src1" },
00401                 { -1, "src2" },
00402                 { 0, 0 }
00403         }
00404 };
00405 
00406 
00407 void AsebaNative_vecsub(AsebaVMState *vm)
00408 {
00409         // variable pos
00410         uint16 dest = AsebaNativePopArg(vm);
00411         uint16 src1 = AsebaNativePopArg(vm);
00412         uint16 src2 = AsebaNativePopArg(vm);
00413         
00414         // variable size
00415         uint16 length = AsebaNativePopArg(vm);
00416         
00417         uint16 i;
00418         for (i = 0; i < length; i++)
00419         {
00420                 vm->variables[dest++] = vm->variables[src1++] - vm->variables[src2++];
00421         }
00422 }
00423 
00424 const AsebaNativeFunctionDescription AsebaNativeDescription_vecsub =
00425 {
00426         "math.sub",
00427         "substracts src2 from src1 to dest, element by element",
00428         {
00429                 { -1, "dest" },
00430                 { -1, "src1" },
00431                 { -1, "src2" },
00432                 { 0, 0 }
00433         }
00434 };
00435 
00436 
00437 void AsebaNative_vecmul(AsebaVMState *vm)
00438 {
00439         // variable pos
00440         uint16 dest = AsebaNativePopArg(vm);
00441         uint16 src1 = AsebaNativePopArg(vm);
00442         uint16 src2 = AsebaNativePopArg(vm);
00443         
00444         // variable size
00445         uint16 length = AsebaNativePopArg(vm);
00446         
00447         uint16 i;
00448         for (i = 0; i < length; i++)
00449         {
00450                 vm->variables[dest++] = vm->variables[src1++] * vm->variables[src2++];
00451         }
00452 }
00453 
00454 const AsebaNativeFunctionDescription AsebaNativeDescription_vecmul =
00455 {
00456         "math.mul",
00457         "multiplies src1 and src2 to dest, element by element",
00458         {
00459                 { -1, "dest" },
00460                 { -1, "src1" },
00461                 { -1, "src2" },
00462                 { 0, 0 }
00463         }
00464 };
00465 
00466 
00467 void AsebaNative_vecdiv(AsebaVMState *vm)
00468 {
00469         // variable pos
00470         uint16 dest = AsebaNativePopArg(vm);
00471         uint16 src1 = AsebaNativePopArg(vm);
00472         uint16 src2 = AsebaNativePopArg(vm);
00473         
00474         // variable size
00475         uint16 length = AsebaNativePopArg(vm);
00476         
00477         uint16 i;
00478         for (i = 0; i < length; i++)
00479         {
00480                 sint32 dividend = (sint32)vm->variables[src1++];
00481                 sint32 divisor = (sint32)vm->variables[src2++];
00482                 
00483                 if (divisor != 0)
00484                 {
00485                         vm->variables[dest++] = (sint16)(dividend / divisor);
00486                 }
00487                 else
00488                 {
00489                         vm->flags = ASEBA_VM_STEP_BY_STEP_MASK;
00490                         AsebaSendMessage(vm, ASEBA_MESSAGE_DIVISION_BY_ZERO, &(vm->pc), sizeof(vm->pc));
00491                         return;
00492                 }
00493         }
00494 }
00495 
00496 const AsebaNativeFunctionDescription AsebaNativeDescription_vecdiv =
00497 {
00498         "math.div",
00499         "divides src1 by src2 to dest, element by element",
00500         {
00501                 { -1, "dest" },
00502                 { -1, "src1" },
00503                 { -1, "src2" },
00504                 { 0, 0 }
00505         }
00506 };
00507 
00508 
00509 void AsebaNative_vecmin(AsebaVMState *vm)
00510 {
00511         // variable pos
00512         uint16 dest = AsebaNativePopArg(vm);
00513         uint16 src1 = AsebaNativePopArg(vm);
00514         uint16 src2 = AsebaNativePopArg(vm);
00515         
00516         // variable size
00517         uint16 length = AsebaNativePopArg(vm);
00518         
00519         uint16 i;
00520         for (i = 0; i < length; i++)
00521         {
00522                 sint16 v1 = vm->variables[src1++];
00523                 sint16 v2 = vm->variables[src2++];
00524                 sint16 res = v1 < v2 ? v1 : v2;
00525                 vm->variables[dest++] = res;
00526         }
00527 }
00528 
00529 const AsebaNativeFunctionDescription AsebaNativeDescription_vecmin =
00530 {
00531         "math.min",
00532         "writes the minimum of src1 and src2 to dest, element by element",
00533         {
00534                 { -1, "dest" },
00535                 { -1, "src1" },
00536                 { -1, "src2" },
00537                 { 0, 0 }
00538         }
00539 };
00540 
00541 
00542 void AsebaNative_vecmax(AsebaVMState *vm)
00543 {
00544         // variable pos
00545         uint16 dest = AsebaNativePopArg(vm);
00546         uint16 src1 = AsebaNativePopArg(vm);
00547         uint16 src2 = AsebaNativePopArg(vm);
00548         
00549         // variable size
00550         uint16 length = AsebaNativePopArg(vm);
00551         
00552         uint16 i;
00553         for (i = 0; i < length; i++)
00554         {
00555                 sint16 v1 = vm->variables[src1++];
00556                 sint16 v2 = vm->variables[src2++];
00557                 sint16 res = v1 > v2 ? v1 : v2;
00558                 vm->variables[dest++] = res;
00559         }
00560 }
00561 
00562 const AsebaNativeFunctionDescription AsebaNativeDescription_vecmax =
00563 {
00564         "math.max",
00565         "writes the maximum of src1 and src2 to dest, element by element",
00566         {
00567                 { -1, "dest" },
00568                 { -1, "src1" },
00569                 { -1, "src2" },
00570                 { 0, 0 }
00571         }
00572 };
00573 
00574 
00575 void AsebaNative_vecdot(AsebaVMState *vm)
00576 {
00577         // variable pos
00578         uint16 dest = AsebaNativePopArg(vm);
00579         uint16 src1 = AsebaNativePopArg(vm);
00580         uint16 src2 = AsebaNativePopArg(vm);
00581         sint16 shift = vm->variables[AsebaNativePopArg(vm)];
00582         
00583         // variable size
00584         uint16 length = AsebaNativePopArg(vm);
00585         sint32 res = 0;
00586         uint16 i;
00587         
00588         if(shift > 32) {
00589                 vm->variables[dest] = 0;
00590                 return;
00591         }
00592 
00593 #ifdef DSP_AVAILABLE
00594         length--;
00595         
00596         CORCONbits.US = 0; // Signed mode
00597         CORCON |= 0b11110001; // 40 bits mode, saturation enable, integer mode.
00598         // Do NOT save the accumulator values, so do NOT USE THIS FUNCTION IN INTERRUPT !       
00599         asm __volatile__ (
00600         "push %[ptr1]           \r\n"
00601         "push %[ptr2]           \r\n"
00602         "clr A\r\n"                                                                     //      A = 0
00603         "mov [%[ptr1]++], w4 \r\n"                                      // Preload ptr
00604         "do %[loop_cnt], 1f     \r\n"                                   //      Iterate loop_cnt time the two following instructions
00605         "mov [%[ptr2]++], w5 \r\n"                                      //      Load w5
00606         "1: mac w4*w5, A, [%[ptr1]]+=2, w4 \r\n"        //      A += w4 * w5, prefetch ptr1 into w4
00607         "pop %[ptr2]            \r\n"
00608         "pop %[ptr1]            \r\n"
00609         : /* No output */
00610         : [loop_cnt] "r" (length), [ptr1] "x" (&vm->variables[src1]), [ptr2] "r" (&vm->variables[src2])
00611         : "cc", "w4", "w5" );
00612         
00613         if(shift > 16) {
00614                 shift -= 16;
00615                 asm __volatile__ ("sftac        A, #16\r\n" : /* No output */ : /* No input */ : "cc");
00616         }
00617         
00618         // Shift and get the output
00619         asm __volatile__ ("sftac A, %[sft]\r\n" : /* No output */ : [sft] "r" (shift) : "cc");
00620         
00621         vm->variables[dest] = ACCAL; // Get the Accumulator low word
00622 #elif defined(__C30__)
00623         for (i= 0; i < length; i++)
00624                 res += __builtin_mulss(vm->variables[src1++], vm->variables[src2++]);
00625         res >>= shift;
00626         vm->variables[dest] = (sint16) res;
00627 #else
00628         for (i = 0; i < length; i++)
00629         {
00630                 res += (sint32)vm->variables[src1++] * (sint32)vm->variables[src2++];
00631         }
00632         res >>= shift;
00633         vm->variables[dest] = (sint16)res;
00634 #endif
00635 }
00636 
00637 const AsebaNativeFunctionDescription AsebaNativeDescription_vecdot =
00638 {
00639         "math.dot",
00640         "writes the dot product of src1 and src2 to dest, after a shift",
00641         {
00642                 { 1, "dest" },
00643                 { -1, "src1" },
00644                 { -1, "src2" },
00645                 { 1, "shift" },
00646                 { 0, 0 }
00647         }
00648 };
00649 
00650 
00651 void AsebaNative_vecstat(AsebaVMState *vm)
00652 {
00653         // variable pos
00654         uint16 src = AsebaNativePopArg(vm);
00655         uint16 min = AsebaNativePopArg(vm);
00656         uint16 max = AsebaNativePopArg(vm);
00657         uint16 mean = AsebaNativePopArg(vm);
00658         
00659         // variable size
00660         uint16 length = AsebaNativePopArg(vm);
00661         sint16 val;
00662         sint32 acc;
00663         uint16 i;
00664         
00665         if (length)
00666         {
00667                 val = vm->variables[src++];
00668                 acc = val;
00669                 vm->variables[min] = val;
00670                 vm->variables[max] = val;
00671                 
00672                 for (i = 1; i < length; i++)
00673                 {
00674                         val = vm->variables[src++];
00675                         if (val < vm->variables[min])
00676                                 vm->variables[min] = val;
00677                         if (val > vm->variables[max])
00678                                 vm->variables[max] = val;
00679                         acc += (sint32)val;
00680                 }
00681                 
00682                 vm->variables[mean] = (sint16)(acc / (sint32)length);
00683         }
00684 }
00685 
00686 const AsebaNativeFunctionDescription AsebaNativeDescription_vecstat =
00687 {
00688         "math.stat",
00689         "performs statistics on src",
00690         {
00691                 { -1, "src" },
00692                 { 1, "min" },
00693                 { 1, "max" },
00694                 { 1, "mean" },
00695                 { 0, 0 }
00696         }
00697 };
00698 
00699 
00700 void AsebaNative_vecargbounds(AsebaVMState *vm)
00701 {
00702         // variable pos
00703         uint16 src = AsebaNativePopArg(vm);
00704         uint16 argmin = AsebaNativePopArg(vm);
00705         uint16 argmax = AsebaNativePopArg(vm);
00706         
00707         // variable size
00708         uint16 length = AsebaNativePopArg(vm);
00709         sint16 min = 32767;
00710         sint16 max = -32768;
00711         sint16 val;
00712         uint16 i;
00713         
00714         if (length)
00715         {
00716                 for (i = 0; i < length; i++)
00717                 {
00718                         val = vm->variables[src++];
00719                         if (val < min)
00720                         {
00721                                 min = val;
00722                                 vm->variables[argmin] = i;
00723                         }
00724                         if (val > max)
00725                         {
00726                                 max = val;
00727                                 vm->variables[argmax] = i;
00728                         }
00729                 }
00730         }
00731 }
00732 
00733 const AsebaNativeFunctionDescription AsebaNativeDescription_vecargbounds =
00734 {
00735         "math.argbounds",
00736         "get the indices (argmin, argmax) of the (minimum, maximum) values of src",
00737         {
00738                 { -1, "src" },
00739                 { 1, "argmin" },
00740                 { 1, "argmax" },
00741                 { 0, 0 }
00742         }
00743 };
00744 
00745 
00746 void AsebaNative_vecsort(AsebaVMState *vm)
00747 {
00748         // variable pos
00749         uint16 src = AsebaNativePopArg(vm);
00750         
00751         // variable size
00752         uint16 length = AsebaNativePopArg(vm);
00753         
00754         aseba_comb_sort(&vm->variables[src], length);
00755 }
00756 
00757 const AsebaNativeFunctionDescription AsebaNativeDescription_vecsort =
00758 {
00759         "math.sort",
00760         "sort array in place",
00761         {
00762                 { -1, "array" },
00763                 { 0, 0 }
00764         }
00765 };
00766 
00767 
00768 void AsebaNative_mathmuldiv(AsebaVMState *vm)
00769 {
00770         // variable pos
00771         uint16 destIndex = AsebaNativePopArg(vm);
00772         uint16 aIndex = AsebaNativePopArg(vm);
00773         uint16 bIndex = AsebaNativePopArg(vm);
00774         uint16 cIndex = AsebaNativePopArg(vm);
00775         
00776         // variable size
00777         uint16 length = AsebaNativePopArg(vm);
00778         
00779         uint16 i;
00780         for (i = 0; i < length; i++)
00781         {
00782                 sint32 a = (sint32)vm->variables[aIndex++];
00783                 sint32 b = (sint32)vm->variables[bIndex++];
00784                 sint32 c = (sint32)vm->variables[cIndex++];
00785                 if (c != 0)
00786                 {
00787                         vm->variables[destIndex++] = (sint16)((a * b) / c);
00788                 }
00789                 else
00790                 {
00791                         vm->flags = ASEBA_VM_STEP_BY_STEP_MASK;
00792                         AsebaSendMessage(vm, ASEBA_MESSAGE_DIVISION_BY_ZERO, &(vm->pc), sizeof(vm->pc));
00793                         return;
00794                 }
00795         }
00796 }
00797 
00798 const AsebaNativeFunctionDescription AsebaNativeDescription_mathmuldiv =
00799 {
00800         "math.muldiv",
00801         "performs dest = (a*b)/c in 32 bits element by element",
00802         {
00803                 { -1, "dest" },
00804                 { -1, "a" },
00805                 { -1, "b" },
00806                 { -1, "c" },
00807                 { 0, 0 }
00808         }
00809 };
00810 
00811 void AsebaNative_mathatan2(AsebaVMState *vm)
00812 {
00813         // variable pos
00814         uint16 destIndex = AsebaNativePopArg(vm);
00815         sint16 yIndex = AsebaNativePopArg(vm);
00816         sint16 xIndex = AsebaNativePopArg(vm);
00817         
00818         // variable size
00819         uint16 length = AsebaNativePopArg(vm);
00820         
00821         uint16 i;
00822         for (i = 0; i < length; i++)
00823         {
00824                 sint16 y = vm->variables[yIndex++];
00825                 sint16 x = vm->variables[xIndex++];
00826                 vm->variables[destIndex++] = aseba_atan2(y, x);
00827         }
00828 }
00829 
00830 const AsebaNativeFunctionDescription AsebaNativeDescription_mathatan2 =
00831 {
00832         "math.atan2",
00833         "performs dest = atan2(y,x) element by element",
00834         {
00835                 { -1, "dest" },
00836                 { -1, "y" },
00837                 { -1, "x" },
00838                 { 0, 0 }
00839         }
00840 };
00841 
00842 void AsebaNative_mathsin(AsebaVMState *vm)
00843 {
00844         // variable pos
00845         uint16 destIndex = AsebaNativePopArg(vm);
00846         sint16 xIndex = AsebaNativePopArg(vm);
00847         
00848         // variable size
00849         uint16 length = AsebaNativePopArg(vm);
00850         
00851         uint16 i;
00852         for (i = 0; i < length; i++)
00853         {
00854                 sint16 x = vm->variables[xIndex++];
00855                 vm->variables[destIndex++] = aseba_sin(x);
00856         }
00857 }
00858 
00859 const AsebaNativeFunctionDescription AsebaNativeDescription_mathsin =
00860 {
00861         "math.sin",
00862         "performs dest = sin(x) element by element",
00863         {
00864                 { -1, "dest" },
00865                 { -1, "x" },
00866                 { 0, 0 }
00867         }
00868 };
00869 
00870 void AsebaNative_mathcos(AsebaVMState *vm)
00871 {
00872         // variable pos
00873         uint16 destIndex = AsebaNativePopArg(vm);
00874         sint16 xIndex = AsebaNativePopArg(vm);
00875         
00876         // variable size
00877         uint16 length = AsebaNativePopArg(vm);
00878         
00879         uint16 i;
00880         for (i = 0; i < length; i++)
00881         {
00882                 sint16 x = vm->variables[xIndex++];
00883                 vm->variables[destIndex++] = aseba_cos(x);
00884         }
00885 }
00886 
00887 const AsebaNativeFunctionDescription AsebaNativeDescription_mathcos =
00888 {
00889         "math.cos",
00890         "performs dest = cos(x) element by element",
00891         {
00892                 { -1, "dest" },
00893                 { -1, "x" },
00894                 { 0, 0 }
00895         }
00896 };
00897 
00898 void AsebaNative_mathrot2(AsebaVMState *vm)
00899 {
00900         // variable pos
00901         uint16 vectOutIndex = AsebaNativePopArg(vm);
00902         uint16 vecInIndex = AsebaNativePopArg(vm);
00903         uint16 angleIndex = AsebaNativePopArg(vm);
00904         
00905         // variables
00906         sint16 x = vm->variables[vecInIndex];
00907         sint16 y = vm->variables[vecInIndex+1];
00908         sint16 a = vm->variables[angleIndex];
00909         
00910         sint16 cos_a = aseba_cos(a);
00911         sint16 sin_a = aseba_sin(a);
00912         
00913         sint16 xp = (sint16)(((sint32)cos_a * (sint32)x - (sint32)sin_a * (sint32)y) >> (sint32)15);
00914         sint16 yp = (sint16)(((sint32)cos_a * (sint32)y + (sint32)sin_a * (sint32)x) >> (sint32)15);
00915         
00916         vm->variables[vectOutIndex] = xp;
00917         vm->variables[vectOutIndex+1] = yp;
00918 }
00919 
00920 const AsebaNativeFunctionDescription AsebaNativeDescription_mathrot2 =
00921 {
00922         "math.rot2",
00923         "rotates v of angle a to dest",
00924         {
00925                 { 2, "dest" },
00926                 { 2, "v" },
00927                 { 1, "a" },
00928                 { 0, 0 }
00929         }
00930 };
00931 
00932 void AsebaNative_mathsqrt(AsebaVMState *vm)
00933 {
00934         // variable pos
00935         uint16 destIndex = AsebaNativePopArg(vm);
00936         sint16 xIndex = AsebaNativePopArg(vm);
00937         
00938         // variable size
00939         uint16 length = AsebaNativePopArg(vm);
00940         
00941         uint16 i;
00942         for (i = 0; i < length; i++)
00943         {
00944                 sint16 x = vm->variables[xIndex++];
00945                 vm->variables[destIndex++] = aseba_sqrt(x);
00946         }
00947 }
00948 
00949 const AsebaNativeFunctionDescription AsebaNativeDescription_mathsqrt =
00950 {
00951         "math.sqrt",
00952         "performs dest = sqrt(x) element by element",
00953         {
00954                 { -1, "dest" },
00955                 { -1, "x" },
00956                 { 0, 0 }
00957         }
00958 };
00959 
00960 void AsebaNative_vecnonzerosequence(AsebaVMState *vm)
00961 {
00962         // variable pos
00963         uint16 dest = AsebaNativePopArg(vm); // output value index
00964         uint16 src = AsebaNativePopArg(vm); // input vector index
00965         uint16 length = AsebaNativePopArg(vm); // length threshold index
00966         
00967         // variable size
00968         uint16 inputLength = AsebaNativePopArg(vm);
00969         sint16 minLength = vm->variables[length]; // length threshold 
00970         
00971         // work variables
00972         uint16 nzFirstZero;
00973         sint16 index;
00974         sint16 seqIndex;
00975         sint16 bestSeqLength;
00976         sint16 bestSeqIndex;
00977         sint16 seqLength;
00978         
00979         // search for a zero, then non-zero
00980         uint16 nzFirstIndex = 0;
00981         while (vm->variables[src + nzFirstIndex] != 0)
00982         {
00983                 ++nzFirstIndex;
00984                 if (nzFirstIndex > inputLength)
00985                 {
00986                         vm->variables[dest] = 0;
00987                         return;
00988                 }
00989         }
00990         nzFirstZero = nzFirstIndex;
00991         while (vm->variables[src + nzFirstIndex] == 0)
00992         {
00993                 ++nzFirstIndex;
00994                 if (nzFirstIndex > inputLength)
00995                 {
00996                         if (nzFirstZero == 0)
00997                         {
00998                                 vm->variables[dest] = -1;
00999                         }
01000                         else
01001                         {
01002                                 uint16 seqLength = nzFirstZero - 1;
01003                                 if (seqLength < minLength)
01004                                         vm->variables[dest] = -1;
01005                                 else
01006                                         vm->variables[dest] = (nzFirstZero - 1) / 2;
01007                         }
01008                         return;
01009                 }
01010         }
01011         
01012         index = 0;
01013         bestSeqLength = 0;
01014         bestSeqIndex = 0;
01015         
01016         while (1)
01017         {
01018                 // scan non-zero sequence
01019                 seqIndex = index;
01020                 while (vm->variables[src + (nzFirstIndex + index) % inputLength] != 0)
01021                 {
01022                         ++index;
01023                         if (index >= inputLength)
01024                                 break;
01025                 }
01026                 // check size
01027                 seqLength = index - seqIndex;
01028                 if (seqLength > bestSeqLength)
01029                 {
01030                         bestSeqLength = seqLength;
01031                         bestSeqIndex = (index + seqIndex) / 2;
01032                 }
01033                 if (index >= inputLength)
01034                         break;
01035                 
01036                 // scan zero sequence
01037                 while (vm->variables[src + (nzFirstIndex + index) % inputLength] == 0)
01038                 {
01039                         ++index;
01040                         if (index >= inputLength)
01041                                 goto doubleBreak;
01042                 }
01043         }
01044         
01045 doubleBreak:
01046         if (bestSeqLength < minLength)
01047         {
01048                 vm->variables[dest] = -1;
01049         }
01050         else
01051         {
01052                 vm->variables[dest] = (nzFirstIndex + bestSeqIndex) % inputLength;
01053         }
01054 }
01055 
01056 
01057 const AsebaNativeFunctionDescription AsebaNativeDescription_vecnonzerosequence =
01058 {
01059         "math.nzseq",
01060         "write to dest the middle index of the largest sequence of non-zero elements from src, -1 if not found or if smaller than minLength",
01061         {
01062                 { 1, "dest" },
01063                 { -1, "src" },
01064                 { 1, "minLength" },
01065                 { 0, 0 }
01066         }
01067 };
01068 
01069 static uint16 rnd_state;
01070 
01071 void AsebaSetRandomSeed(uint16 seed)
01072 {
01073         rnd_state = seed;
01074 }
01075 
01076 uint16 AsebaGetRandom()
01077 {
01078         rnd_state = 25173 * rnd_state + 13849;
01079         return rnd_state;
01080 }
01081 
01082 void AsebaNative_rand(AsebaVMState *vm)
01083 {
01084         // variable pos
01085         uint16 destIndex = AsebaNativePopArg(vm);
01086         
01087         // variable size
01088         uint16 length = AsebaNativePopArg(vm);
01089         
01090         uint16 i;
01091         for (i = 0; i < length; i++)
01092         {
01093                 vm->variables[destIndex++] = (sint16)AsebaGetRandom();
01094         }
01095 }
01096 
01097 const AsebaNativeFunctionDescription AsebaNativeDescription_rand =
01098 {
01099         "math.rand",
01100         "fill array with random values",
01101         {
01102                 { -1, "dest" },
01103                 { 0, 0 }
01104         }
01105 };
01106 
01107 


aseba
Author(s): Stéphane Magnenat
autogenerated on Sun Oct 5 2014 23:46:38