00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "../common/consts.h"
00024 #include "../common/types.h"
00025 #include "natives.h"
00026 #include <string.h>
00027
00028 #include <assert.h>
00029
00030 #if defined(__dsPIC30F__)
00031 #include <p30fxxxx.h>
00032 #define DSP_AVAILABLE
00033 #elif defined(__dsPIC33F__)
00034 #include <p33Fxxxx.h>
00035 #define DSP_AVAILABLE
00036 #elif defined(__PIC24H__)
00037 #include <p24Hxxxx.h>
00038 #endif
00039
00040
00041
00049
00050
00051
00052
00053 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 };
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 sint16 aseba_atan2(sint16 y, sint16 x)
00072 {
00073 if (y == 0)
00074 {
00075 if (x >= 0)
00076 return 0;
00077 else if (x < 0)
00078 return -32768;
00079 }
00080
00081 {
00082 sint16 res;
00083 sint16 ax = abs(x);
00084 sint16 ay = abs(y);
00085 if (x == 0)
00086 {
00087 res = 16384;
00088 }
00089 else
00090 {
00091 sint32 value = (((sint32)ay << 16)/(sint32)(ax));
00092 sint16 fb1 = 0;
00093
00094
00095 #ifdef __C30__
00096
00097
00098 asm ("ff1l %[word], %[b]" : [b] "=r" (fb1) : [word] "r" ((int) (value >> 16)) : "cc");
00099 if(fb1)
00100 fb1 = fb1 + 16 - 1;
00101 else {
00102 asm ("ff1l %[word], %[b]" : [b] "=r" (fb1) : [word] "r" ((int) value) : "cc");
00103 if(fb1)
00104 fb1--;
00105 }
00106
00107 #else
00108 sint16 fb1_counter;
00109 for (fb1_counter = 0; fb1_counter < 32; fb1_counter++)
00110 if ((value >> (sint32)fb1_counter) != 0)
00111 fb1 = fb1_counter;
00112 #endif
00113 {
00114
00115 sint16 index = fb1 - 12;
00116 if (index < 0)
00117 {
00118
00119 res = (sint16)(((sint32)aseba_atan_table[0] * value) >> 12);
00120 }
00121 else
00122 {
00123 sint32 subprecision_rest = value - (1 << (sint32)fb1);
00124 sint16 to_shift = fb1 - 8;
00125 sint16 subprecision_index = (sint16)(subprecision_rest >> (sint32)to_shift);
00126 sint16 bin = subprecision_index >> 5;
00127 sint16 delta = subprecision_index & 0x1f;
00128 res = (sint16)(((sint32)aseba_atan_table[index*8 + bin] * (sint32)(32 - delta) + (sint32)aseba_atan_table[index*8 + bin + 1] * (sint32)delta) >> 5);
00129 }
00130
00131
00132 if (x < 0)
00133 res = 32768 - res;
00134 }
00135 }
00136
00137 if (y > 0)
00138 return res;
00139 else
00140 return -res;
00141 }
00142 }
00143
00144
00145 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, };
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157 sint16 aseba_sin(sint16 angle)
00158 {
00159 sint16 index;
00160 sint16 subIndex;
00161 sint16 invert;
00162 sint16 lookupAngle;
00163 if (angle < 0)
00164 {
00165 if (angle < -16384)
00166 lookupAngle = 32768 + angle;
00167 else if (angle > -16384)
00168 lookupAngle = -angle;
00169 else
00170 return -32767;
00171 invert = 1;
00172 }
00173 else
00174 {
00175 if (angle > 16384)
00176 lookupAngle = 32767 - angle + 1;
00177 else if (angle < 16484)
00178 lookupAngle = angle;
00179 else
00180 return 32767;
00181 invert = 0;
00182 }
00183
00184 index = lookupAngle >> 7;
00185 subIndex = lookupAngle & 0x7f;
00186
00187 {
00188 sint16 result = (sint16)(((sint32)aseba_sin_table[index] * (sint32)(128-subIndex) + (sint32)aseba_sin_table[index+1] * (sint32)(subIndex)) >> 7);
00189
00190 if (invert)
00191 return -result;
00192 else
00193 return result;
00194 }
00195 }
00196
00197
00198 sint16 aseba_cos(sint16 angle)
00199 {
00200 return aseba_sin(16384 + angle);
00201 }
00202
00203
00204 sint16 aseba_sqrt(sint16 num)
00205 {
00206 sint16 op = num;
00207 sint16 res = 0;
00208 sint16 one = 1 << 14;
00209
00210 while(one > op)
00211 one >>= 2;
00212
00213 while(one != 0)
00214 {
00215 if (op >= res + one)
00216 {
00217 op -= res + one;
00218 res = (res >> 1) + one;
00219 }
00220 else
00221 {
00222 res >>= 1;
00223 }
00224 one >>= 2;
00225 }
00226 return res;
00227 }
00228
00229
00230
00231
00232 void AsebaNative_veccopy(AsebaVMState *vm)
00233 {
00234
00235 uint16 dest = AsebaNativePopArg(vm);
00236 uint16 src = AsebaNativePopArg(vm);
00237
00238
00239 uint16 length = AsebaNativePopArg(vm);
00240
00241 uint16 i;
00242
00243 for (i = 0; i < length; i++)
00244 {
00245 vm->variables[dest++] = vm->variables[src++];
00246 }
00247 }
00248
00249 const AsebaNativeFunctionDescription AsebaNativeDescription_veccopy =
00250 {
00251 "math.copy",
00252 "copies src to dest element by element",
00253 {
00254 { -1, "dest" },
00255 { -1, "src" },
00256 { 0, 0 }
00257 }
00258 };
00259
00260
00261 void AsebaNative_vecfill(AsebaVMState *vm)
00262 {
00263
00264 uint16 dest = AsebaNativePopArg(vm);
00265 uint16 value = AsebaNativePopArg(vm);
00266
00267
00268 uint16 length = AsebaNativePopArg(vm);
00269
00270 uint16 i;
00271
00272 for (i = 0; i < length; i++)
00273 {
00274 vm->variables[dest++] = vm->variables[value];
00275 }
00276 }
00277
00278 const AsebaNativeFunctionDescription AsebaNativeDescription_vecfill =
00279 {
00280 "math.fill",
00281 "fills dest with constant value",
00282 {
00283 { -1, "dest" },
00284 { 1, "value" },
00285 { 0, 0 }
00286 }
00287 };
00288
00289
00290 void AsebaNative_vecaddscalar(AsebaVMState *vm)
00291 {
00292
00293 uint16 dest = AsebaNativePopArg(vm);
00294 uint16 src = AsebaNativePopArg(vm);
00295 uint16 scalar = AsebaNativePopArg(vm);
00296
00297
00298 uint16 length = AsebaNativePopArg(vm);
00299
00300 const sint16 scalarValue = vm->variables[scalar];
00301 uint16 i;
00302 for (i = 0; i < length; i++)
00303 {
00304 vm->variables[dest++] = vm->variables[src++] + scalarValue;
00305 }
00306 }
00307
00308 const AsebaNativeFunctionDescription AsebaNativeDescription_vecaddscalar =
00309 {
00310 "math.addscalar",
00311 "add scalar to each element of src and store result to dest",
00312 {
00313 { -1, "dest" },
00314 { -1, "src" },
00315 { 1, "scalar" },
00316 { 0, 0 }
00317 }
00318 };
00319
00320
00321 void AsebaNative_vecadd(AsebaVMState *vm)
00322 {
00323
00324 uint16 dest = AsebaNativePopArg(vm);
00325 uint16 src1 = AsebaNativePopArg(vm);
00326 uint16 src2 = AsebaNativePopArg(vm);
00327
00328
00329 uint16 length = AsebaNativePopArg(vm);
00330
00331 uint16 i;
00332 for (i = 0; i < length; i++)
00333 {
00334 vm->variables[dest++] = vm->variables[src1++] + vm->variables[src2++];
00335 }
00336 }
00337
00338 const AsebaNativeFunctionDescription AsebaNativeDescription_vecadd =
00339 {
00340 "math.add",
00341 "adds src1 and src2 to dest, element by element",
00342 {
00343 { -1, "dest" },
00344 { -1, "src1" },
00345 { -1, "src2" },
00346 { 0, 0 }
00347 }
00348 };
00349
00350
00351 void AsebaNative_vecsub(AsebaVMState *vm)
00352 {
00353
00354 uint16 dest = AsebaNativePopArg(vm);
00355 uint16 src1 = AsebaNativePopArg(vm);
00356 uint16 src2 = AsebaNativePopArg(vm);
00357
00358
00359 uint16 length = AsebaNativePopArg(vm);
00360
00361 uint16 i;
00362 for (i = 0; i < length; i++)
00363 {
00364 vm->variables[dest++] = vm->variables[src1++] - vm->variables[src2++];
00365 }
00366 }
00367
00368 const AsebaNativeFunctionDescription AsebaNativeDescription_vecsub =
00369 {
00370 "math.sub",
00371 "substracts src2 from src1 to dest, element by element",
00372 {
00373 { -1, "dest" },
00374 { -1, "src1" },
00375 { -1, "src2" },
00376 { 0, 0 }
00377 }
00378 };
00379
00380
00381 void AsebaNative_vecmul(AsebaVMState *vm)
00382 {
00383
00384 uint16 dest = AsebaNativePopArg(vm);
00385 uint16 src1 = AsebaNativePopArg(vm);
00386 uint16 src2 = AsebaNativePopArg(vm);
00387
00388
00389 uint16 length = AsebaNativePopArg(vm);
00390
00391 uint16 i;
00392 for (i = 0; i < length; i++)
00393 {
00394 vm->variables[dest++] = vm->variables[src1++] * vm->variables[src2++];
00395 }
00396 }
00397
00398 const AsebaNativeFunctionDescription AsebaNativeDescription_vecmul =
00399 {
00400 "math.mul",
00401 "multiplies src1 and src2 to dest, element by element",
00402 {
00403 { -1, "dest" },
00404 { -1, "src1" },
00405 { -1, "src2" },
00406 { 0, 0 }
00407 }
00408 };
00409
00410
00411 void AsebaNative_vecdiv(AsebaVMState *vm)
00412 {
00413
00414 uint16 dest = AsebaNativePopArg(vm);
00415 uint16 src1 = AsebaNativePopArg(vm);
00416 uint16 src2 = AsebaNativePopArg(vm);
00417
00418
00419 uint16 length = AsebaNativePopArg(vm);
00420
00421 uint16 i;
00422 for (i = 0; i < length; i++)
00423 {
00424 sint32 dividend = (sint32)vm->variables[src1++];
00425 sint32 divisor = (sint32)vm->variables[src2++];
00426
00427 if (divisor != 0)
00428 {
00429 vm->variables[dest++] = (sint16)(dividend / divisor);
00430 }
00431 else
00432 {
00433 vm->flags = ASEBA_VM_STEP_BY_STEP_MASK;
00434 AsebaSendMessage(vm, ASEBA_MESSAGE_DIVISION_BY_ZERO, &(vm->pc), sizeof(vm->pc));
00435 return;
00436 }
00437 }
00438 }
00439
00440 const AsebaNativeFunctionDescription AsebaNativeDescription_vecdiv =
00441 {
00442 "math.div",
00443 "divides src1 by src2 to dest, element by element",
00444 {
00445 { -1, "dest" },
00446 { -1, "src1" },
00447 { -1, "src2" },
00448 { 0, 0 }
00449 }
00450 };
00451
00452
00453 void AsebaNative_vecmin(AsebaVMState *vm)
00454 {
00455
00456 uint16 dest = AsebaNativePopArg(vm);
00457 uint16 src1 = AsebaNativePopArg(vm);
00458 uint16 src2 = AsebaNativePopArg(vm);
00459
00460
00461 uint16 length = AsebaNativePopArg(vm);
00462
00463 uint16 i;
00464 for (i = 0; i < length; i++)
00465 {
00466 sint16 v1 = vm->variables[src1++];
00467 sint16 v2 = vm->variables[src2++];
00468 sint16 res = v1 < v2 ? v1 : v2;
00469 vm->variables[dest++] = res;
00470 }
00471 }
00472
00473 const AsebaNativeFunctionDescription AsebaNativeDescription_vecmin =
00474 {
00475 "math.min",
00476 "writes the minimum of src1 and src2 to dest, element by element",
00477 {
00478 { -1, "dest" },
00479 { -1, "src1" },
00480 { -1, "src2" },
00481 { 0, 0 }
00482 }
00483 };
00484
00485
00486 void AsebaNative_vecmax(AsebaVMState *vm)
00487 {
00488
00489 uint16 dest = AsebaNativePopArg(vm);
00490 uint16 src1 = AsebaNativePopArg(vm);
00491 uint16 src2 = AsebaNativePopArg(vm);
00492
00493
00494 uint16 length = AsebaNativePopArg(vm);
00495
00496 uint16 i;
00497 for (i = 0; i < length; i++)
00498 {
00499 sint16 v1 = vm->variables[src1++];
00500 sint16 v2 = vm->variables[src2++];
00501 sint16 res = v1 > v2 ? v1 : v2;
00502 vm->variables[dest++] = res;
00503 }
00504 }
00505
00506 const AsebaNativeFunctionDescription AsebaNativeDescription_vecmax =
00507 {
00508 "math.max",
00509 "writes the maximum of src1 and src2 to dest, element by element",
00510 {
00511 { -1, "dest" },
00512 { -1, "src1" },
00513 { -1, "src2" },
00514 { 0, 0 }
00515 }
00516 };
00517
00518
00519
00520 void AsebaNative_vecdot(AsebaVMState *vm)
00521 {
00522
00523 uint16 dest = AsebaNativePopArg(vm);
00524 uint16 src1 = AsebaNativePopArg(vm);
00525 uint16 src2 = AsebaNativePopArg(vm);
00526 sint16 shift = vm->variables[AsebaNativePopArg(vm)];
00527
00528
00529 uint16 length = AsebaNativePopArg(vm);
00530 sint32 res = 0;
00531 uint16 i;
00532
00533 if(shift > 32) {
00534 vm->variables[dest] = 0;
00535 return;
00536 }
00537
00538 #ifdef DSP_AVAILABLE
00539 length--;
00540
00541 CORCONbits.US = 0;
00542 CORCON |= 0b11110001;
00543
00544 asm __volatile__ (
00545 "push %[ptr1] \r\n"
00546 "push %[ptr2] \r\n"
00547 "clr A\r\n"
00548 "mov [%[ptr1]++], w4 \r\n"
00549 "do %[loop_cnt], 1f \r\n"
00550 "mov [%[ptr2]++], w5 \r\n"
00551 "1: mac w4*w5, A, [%[ptr1]]+=2, w4 \r\n"
00552 "pop %[ptr2] \r\n"
00553 "pop %[ptr1] \r\n"
00554 :
00555 : [loop_cnt] "r" (length), [ptr1] "x" (&vm->variables[src1]), [ptr2] "r" (&vm->variables[src2])
00556 : "cc", "w4", "w5" );
00557
00558 if(shift > 16) {
00559 shift -= 16;
00560 asm __volatile__ ("sftac A, #16\r\n" : : : "cc");
00561 }
00562
00563
00564 asm __volatile__ ("sftac A, %[sft]\r\n" : : [sft] "r" (shift) : "cc");
00565
00566 vm->variables[dest] = ACCAL;
00567 #elif defined(__C30__)
00568 for (i= 0; i < length; i++)
00569 res += __builtin_mulss(vm->variables[src1++], vm->variables[src2++]);
00570 res >>= shift;
00571 vm->variables[dest] = (sint16) res;
00572 #else
00573 for (i = 0; i < length; i++)
00574 {
00575 res += (sint32)vm->variables[src1++] * (sint32)vm->variables[src2++];
00576 }
00577 res >>= shift;
00578 vm->variables[dest] = (sint16)res;
00579 #endif
00580 }
00581
00582 const AsebaNativeFunctionDescription AsebaNativeDescription_vecdot =
00583 {
00584 "math.dot",
00585 "writes the dot product of src1 and src2 to dest, after a shift",
00586 {
00587 { 1, "dest" },
00588 { -1, "src1" },
00589 { -1, "src2" },
00590 { 1, "shift" },
00591 { 0, 0 }
00592 }
00593 };
00594
00595
00596 void AsebaNative_vecstat(AsebaVMState *vm)
00597 {
00598
00599 uint16 src = AsebaNativePopArg(vm);
00600 uint16 min = AsebaNativePopArg(vm);
00601 uint16 max = AsebaNativePopArg(vm);
00602 uint16 mean = AsebaNativePopArg(vm);
00603
00604
00605 uint16 length = AsebaNativePopArg(vm);
00606 sint16 val;
00607 sint32 acc;
00608 uint16 i;
00609
00610 if (length)
00611 {
00612 val = vm->variables[src++];
00613 acc = val;
00614 vm->variables[min] = val;
00615 vm->variables[max] = val;
00616
00617 for (i = 1; i < length; i++)
00618 {
00619 val = vm->variables[src++];
00620 if (val < vm->variables[min])
00621 vm->variables[min] = val;
00622 if (val > vm->variables[max])
00623 vm->variables[max] = val;
00624 acc += (sint32)val;
00625 }
00626
00627 vm->variables[mean] = (sint16)(acc / (sint32)length);
00628 }
00629 }
00630
00631 const AsebaNativeFunctionDescription AsebaNativeDescription_vecstat =
00632 {
00633 "math.stat",
00634 "performs statistics on src",
00635 {
00636 { -1, "src" },
00637 { 1, "min" },
00638 { 1, "max" },
00639 { 1, "mean" },
00640 { 0, 0 }
00641 }
00642 };
00643
00644 void AsebaNative_mathmuldiv(AsebaVMState *vm)
00645 {
00646
00647 uint16 destIndex = AsebaNativePopArg(vm);
00648 sint16 aIndex = AsebaNativePopArg(vm);
00649 sint16 bIndex = AsebaNativePopArg(vm);
00650 sint16 cIndex = AsebaNativePopArg(vm);
00651
00652
00653 uint16 length = AsebaNativePopArg(vm);
00654
00655 uint16 i;
00656 for (i = 0; i < length; i++)
00657 {
00658 sint32 a = (sint32)vm->variables[aIndex++];
00659 sint32 b = (sint32)vm->variables[bIndex++];
00660 sint32 c = (sint32)vm->variables[cIndex++];
00661 if (c != 0)
00662 {
00663 vm->variables[destIndex++] = (sint16)((a * b) / c);
00664 }
00665 else
00666 {
00667 vm->flags = ASEBA_VM_STEP_BY_STEP_MASK;
00668 AsebaSendMessage(vm, ASEBA_MESSAGE_DIVISION_BY_ZERO, &(vm->pc), sizeof(vm->pc));
00669 return;
00670 }
00671 }
00672 }
00673
00674 const AsebaNativeFunctionDescription AsebaNativeDescription_mathmuldiv =
00675 {
00676 "math.muldiv",
00677 "performs dest = (a*b)/c in 32 bits element by element",
00678 {
00679 { -1, "dest" },
00680 { -1, "a" },
00681 { -1, "b" },
00682 { -1, "c" },
00683 { 0, 0 }
00684 }
00685 };
00686
00687 void AsebaNative_mathatan2(AsebaVMState *vm)
00688 {
00689
00690 uint16 destIndex = AsebaNativePopArg(vm);
00691 sint16 yIndex = AsebaNativePopArg(vm);
00692 sint16 xIndex = AsebaNativePopArg(vm);
00693
00694
00695 uint16 length = AsebaNativePopArg(vm);
00696
00697 uint16 i;
00698 for (i = 0; i < length; i++)
00699 {
00700 sint16 y = vm->variables[yIndex++];
00701 sint16 x = vm->variables[xIndex++];
00702 vm->variables[destIndex++] = aseba_atan2(y, x);
00703 }
00704 }
00705
00706 const AsebaNativeFunctionDescription AsebaNativeDescription_mathatan2 =
00707 {
00708 "math.atan2",
00709 "performs dest = atan2(y,x) element by element",
00710 {
00711 { -1, "dest" },
00712 { -1, "y" },
00713 { -1, "x" },
00714 { 0, 0 }
00715 }
00716 };
00717
00718 void AsebaNative_mathsin(AsebaVMState *vm)
00719 {
00720
00721 uint16 destIndex = AsebaNativePopArg(vm);
00722 sint16 xIndex = AsebaNativePopArg(vm);
00723
00724
00725 uint16 length = AsebaNativePopArg(vm);
00726
00727 uint16 i;
00728 for (i = 0; i < length; i++)
00729 {
00730 sint16 x = vm->variables[xIndex++];
00731 vm->variables[destIndex++] = aseba_sin(x);
00732 }
00733 }
00734
00735 const AsebaNativeFunctionDescription AsebaNativeDescription_mathsin =
00736 {
00737 "math.sin",
00738 "performs dest = sin(x) element by element",
00739 {
00740 { -1, "dest" },
00741 { -1, "x" },
00742 { 0, 0 }
00743 }
00744 };
00745
00746 void AsebaNative_mathcos(AsebaVMState *vm)
00747 {
00748
00749 uint16 destIndex = AsebaNativePopArg(vm);
00750 sint16 xIndex = AsebaNativePopArg(vm);
00751
00752
00753 uint16 length = AsebaNativePopArg(vm);
00754
00755 uint16 i;
00756 for (i = 0; i < length; i++)
00757 {
00758 sint16 x = vm->variables[xIndex++];
00759 vm->variables[destIndex++] = aseba_cos(x);
00760 }
00761 }
00762
00763 const AsebaNativeFunctionDescription AsebaNativeDescription_mathcos =
00764 {
00765 "math.cos",
00766 "performs dest = cos(x) element by element",
00767 {
00768 { -1, "dest" },
00769 { -1, "x" },
00770 { 0, 0 }
00771 }
00772 };
00773
00774 void AsebaNative_mathrot2(AsebaVMState *vm)
00775 {
00776
00777 uint16 vectOutIndex = AsebaNativePopArg(vm);
00778 uint16 vecInIndex = AsebaNativePopArg(vm);
00779 uint16 angleIndex = AsebaNativePopArg(vm);
00780
00781
00782 sint16 x = vm->variables[vecInIndex];
00783 sint16 y = vm->variables[vecInIndex+1];
00784 sint16 a = vm->variables[angleIndex];
00785
00786 sint16 cos_a = aseba_cos(a);
00787 sint16 sin_a = aseba_sin(a);
00788
00789 sint16 xp = (sint16)(((sint32)cos_a * (sint32)x - (sint32)sin_a * (sint32)y) >> (sint32)15);
00790 sint16 yp = (sint16)(((sint32)cos_a * (sint32)y + (sint32)sin_a * (sint32)x) >> (sint32)15);
00791
00792 vm->variables[vectOutIndex] = xp;
00793 vm->variables[vectOutIndex+1] = yp;
00794 }
00795
00796 const AsebaNativeFunctionDescription AsebaNativeDescription_mathrot2 =
00797 {
00798 "math.rot2",
00799 "rotates v of angle a to dest",
00800 {
00801 { 2, "dest" },
00802 { 2, "v" },
00803 { 1, "a" },
00804 { 0, 0 }
00805 }
00806 };
00807
00808 void AsebaNative_mathsqrt(AsebaVMState *vm)
00809 {
00810
00811 uint16 destIndex = AsebaNativePopArg(vm);
00812 sint16 xIndex = AsebaNativePopArg(vm);
00813
00814
00815 uint16 length = AsebaNativePopArg(vm);
00816
00817 uint16 i;
00818 for (i = 0; i < length; i++)
00819 {
00820 sint16 x = vm->variables[xIndex++];
00821 vm->variables[destIndex++] = aseba_sqrt(x);
00822 }
00823 }
00824
00825 const AsebaNativeFunctionDescription AsebaNativeDescription_mathsqrt =
00826 {
00827 "math.sqrt",
00828 "performs dest = sqrt(x) element by element",
00829 {
00830 { -1, "dest" },
00831 { -1, "x" },
00832 { 0, 0 }
00833 }
00834 };
00835
00836 void AsebaNative_vecnonzerosequence(AsebaVMState *vm)
00837 {
00838
00839 uint16 dest = AsebaNativePopArg(vm);
00840 uint16 src = AsebaNativePopArg(vm);
00841 uint16 length = AsebaNativePopArg(vm);
00842
00843
00844 uint16 inputLength = AsebaNativePopArg(vm);
00845 sint16 minLength = vm->variables[length];
00846
00847
00848 uint16 nzFirstZero;
00849 sint16 index;
00850 sint16 seqIndex;
00851 sint16 bestSeqLength;
00852 sint16 bestSeqIndex;
00853 sint16 seqLength;
00854
00855
00856 uint16 nzFirstIndex = 0;
00857 while (vm->variables[src + nzFirstIndex] != 0)
00858 {
00859 ++nzFirstIndex;
00860 if (nzFirstIndex > inputLength)
00861 {
00862 vm->variables[dest] = 0;
00863 return;
00864 }
00865 }
00866 nzFirstZero = nzFirstIndex;
00867 while (vm->variables[src + nzFirstIndex] == 0)
00868 {
00869 ++nzFirstIndex;
00870 if (nzFirstIndex > inputLength)
00871 {
00872 if (nzFirstZero == 0)
00873 {
00874 vm->variables[dest] = -1;
00875 }
00876 else
00877 {
00878 uint16 seqLength = nzFirstZero - 1;
00879 if (seqLength < minLength)
00880 vm->variables[dest] = -1;
00881 else
00882 vm->variables[dest] = (nzFirstZero - 1) / 2;
00883 }
00884 return;
00885 }
00886 }
00887
00888 index = 0;
00889 bestSeqLength = 0;
00890 bestSeqIndex = 0;
00891
00892 while (1)
00893 {
00894
00895 seqIndex = index;
00896 while (vm->variables[src + (nzFirstIndex + index) % inputLength] != 0)
00897 {
00898 ++index;
00899 if (index >= inputLength)
00900 break;
00901 }
00902
00903 seqLength = index - seqIndex;
00904 if (seqLength > bestSeqLength)
00905 {
00906 bestSeqLength = seqLength;
00907 bestSeqIndex = (index + seqIndex) / 2;
00908 }
00909 if (index >= inputLength)
00910 break;
00911
00912
00913 while (vm->variables[src + (nzFirstIndex + index) % inputLength] == 0)
00914 {
00915 ++index;
00916 if (index >= inputLength)
00917 goto doubleBreak;
00918 }
00919 }
00920
00921 doubleBreak:
00922 if (bestSeqLength < minLength)
00923 {
00924 vm->variables[dest] = -1;
00925 }
00926 else
00927 {
00928 vm->variables[dest] = (nzFirstIndex + bestSeqIndex) % inputLength;
00929 }
00930 }
00931
00932
00933 const AsebaNativeFunctionDescription AsebaNativeDescription_vecnonzerosequence =
00934 {
00935 "math.nzseq",
00936 "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",
00937 {
00938 { 1, "dest" },
00939 { -1, "src" },
00940 { 1, "minLength" },
00941 { 0, 0 }
00942 }
00943 };
00944