From d17b33131c14864bd1eae275f49a3f148e21cf29 Mon Sep 17 00:00:00 2001 From: Leo Chan Date: Thu, 22 Oct 2020 01:53:21 -0400 Subject: Squashed commit of the sb-vbs branch. Includes the SD-VBS benchmarks modified to: - Use libextra to loop as realtime jobs - Preallocate memory before starting their main computation - Accept input via stdin instead of via argc Does not include the SD-VBS matlab code. Fixes libextra execution in LITMUS^RT. --- SD-VBS/benchmarks/localization/src/c/eul2quat.c | 50 ++ .../benchmarks/localization/src/c/generateSample.c | 88 ++++ .../localization/src/c/get3DGaussianProb.c | 49 ++ SD-VBS/benchmarks/localization/src/c/input.txt | 8 + .../benchmarks/localization/src/c/localization.h | 24 + SD-VBS/benchmarks/localization/src/c/mcl.c | 34 ++ SD-VBS/benchmarks/localization/src/c/quat2eul.c | 47 ++ SD-VBS/benchmarks/localization/src/c/quatConj.c | 32 ++ SD-VBS/benchmarks/localization/src/c/quatMul.c | 55 +++ SD-VBS/benchmarks/localization/src/c/quatRot.c | 47 ++ .../benchmarks/localization/src/c/readSensorData.c | 63 +++ .../localization/src/c/script_localization.c | 534 +++++++++++++++++++++ .../benchmarks/localization/src/c/weightedSample.c | 37 ++ 13 files changed, 1068 insertions(+) create mode 100644 SD-VBS/benchmarks/localization/src/c/eul2quat.c create mode 100644 SD-VBS/benchmarks/localization/src/c/generateSample.c create mode 100644 SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c create mode 100644 SD-VBS/benchmarks/localization/src/c/input.txt create mode 100644 SD-VBS/benchmarks/localization/src/c/localization.h create mode 100644 SD-VBS/benchmarks/localization/src/c/mcl.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quat2eul.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quatConj.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quatMul.c create mode 100644 SD-VBS/benchmarks/localization/src/c/quatRot.c create mode 100644 SD-VBS/benchmarks/localization/src/c/readSensorData.c create mode 100644 SD-VBS/benchmarks/localization/src/c/script_localization.c create mode 100644 SD-VBS/benchmarks/localization/src/c/weightedSample.c (limited to 'SD-VBS/benchmarks/localization/src') diff --git a/SD-VBS/benchmarks/localization/src/c/eul2quat.c b/SD-VBS/benchmarks/localization/src/c/eul2quat.c new file mode 100644 index 0000000..bea75cf --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/eul2quat.c @@ -0,0 +1,50 @@ +/******************************** +Author: Sravanthi Kota Venkata +********************************/ + +#include +#include +#include "localization.h" + +F2D* eul2quat(F2D* angle) +{ + F2D *ret; + F2D *x, *y, *z; + int k, i, j; + int rows, cols; + + rows = angle->height; + cols = angle->width; + + x = fDeepCopyRange(angle, 0, angle->height, 0, 1); + y = fDeepCopyRange(angle, 0, angle->height, 1, 1); + z = fDeepCopyRange(angle, 0, angle->height, 2, 1); + + ret = fSetArray(x->height, 4, 0); + + for(i=0; i +#include +#include "localization.h" + +void generateSample(F2D *w, F2D *quat, F2D *vel, F2D *pos) +{ + int rows, cols, i, j, index; + I2D *sampleXId; + F2D *retQuat, *retVel, *retPos; + + sampleXId = weightedSample(w); + + rows = sampleXId->height; + cols = sampleXId->width; + + if(cols > 1) + printf("ERROR: Cols more than 1.. Handle this case \n"); + + retQuat = fSetArray(quat->height, quat->width, 0); + retVel = fSetArray(vel->height, vel->width, 0); + retPos = fSetArray(pos->height, pos->width, 0); + + for(i=0; iwidth; j++) + { + subsref(retQuat,i,j) = subsref(quat,index,j); + } + } + + for(i=0; iwidth; j++) + { + subsref(retVel,i,j) = subsref(vel,index,j); + } + } + + for(i=0; iwidth; j++) + { + subsref(retPos,i,j) = subsref(pos,index,j); + } + } + + for(i=0; iheight; i++) + { + for(j=0; jwidth; j++) + { + subsref(quat,i,j) = subsref(retQuat,i,j); + } + } + + for(i=0; iheight; i++) + { + for(j=0; jwidth; j++) + { + subsref(vel,i,j) = subsref(retVel,i,j); + } + } + + for(i=0; iheight; i++) + { + for(j=0; jwidth; j++) + { + subsref(pos,i,j) = subsref(retPos,i,j); + } + } + + fFreeHandle(retQuat); + fFreeHandle(retVel); + fFreeHandle(retPos); + iFreeHandle(sampleXId); + + return; +} + + + + diff --git a/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c b/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c new file mode 100644 index 0000000..58c4475 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/get3DGaussianProb.c @@ -0,0 +1,49 @@ +/******************************** +Author: Sravanthi Kota Venkata +********************************/ + +#include +#include +#include "localization.h" + +F2D* get3DGaussianProb( F2D* data, F2D* mean, F2D* A) +{ + F2D *p, *diff, *temp1, *temp2, *mt; + float temp; + int n_data, n_channel; + int i, j, k; + F2D* t; + float pi = 3.1412; + + n_data = data->height; + n_channel = data->width; + + t = fSetArray(n_data, 1, 1); + + mt = fMtimes(t, mean); + diff = fMinus( data, mt); + p = fSetArray(diff->height, 1, 0); + + temp = sqrt(1.0/(pow(2*pi, n_channel))); + temp2 = randWrapper(diff->height,1); + + j = (temp2->height*temp2->width); + for(i=0; i +#include +#include "localization.h" + +F2D* mcl(F2D* x, F2D* sData, F2D* invConv) +{ + int i, j; + F2D *retW, *retX, *sum; + float sumVal; + + retX = fDeepCopy(x); + retW = get3DGaussianProb(retX, sData, invConv); + sum = fSum(retW); + if(sum->height == 1 && sum->width ==1) + { + sumVal = asubsref(sum,0); + for(i=0; iheight; i++) + for(j=0; jwidth; j++) + subsref(retW,i,j) = subsref(retW,i,j)/sumVal; + } + else + retW = fMdivide(retW, sum); + + fFreeHandle(retX); + fFreeHandle(sum); + + return retW; +} + + diff --git a/SD-VBS/benchmarks/localization/src/c/quat2eul.c b/SD-VBS/benchmarks/localization/src/c/quat2eul.c new file mode 100644 index 0000000..bc581c9 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/quat2eul.c @@ -0,0 +1,47 @@ +/******************************** +Author: Sravanthi Kota Venkata +********************************/ + +#include +#include +#include "localization.h" +#include + +F2D* quat2eul(F2D* quat) +{ + F2D *retEul; + int i, j, k; + int rows, cols; + + rows = quat->height; + cols = quat->width; + + retEul = fSetArray(rows, 3, 0); + + for(i=0; i +#include +#include "localization.h" + +F2D* quatConj(F2D* a) +{ + F2D* retQuat; + int rows, cols; + int i, j, k; + + rows = a->height; + cols = a->width; + retQuat = fSetArray(rows, 4, 0); + + for(i=0; i +#include +#include "localization.h" + +F2D* quatMul(F2D* a, F2D* b) +{ + int ra, ca, rb, cb; + F2D *ret; + int i, j, k=0; + + ra = a->height; + ca = a->width; + + rb = b->height; + cb = b->width; + + ret = fSetArray(ra, 4, 0); + + j = 0; + for(i=0; i +#include +#include "localization.h" + +F2D* quatRot(F2D* vec, F2D* rQuat) +{ + F2D *ret; + int nr, i, j, k, rows, cols; + F2D *tv, *vQuat, *temp, *temp1; + F2D *retVec; + + nr = vec->height; + tv = fSetArray(nr, 1, 0); + vQuat = fHorzcat(tv, vec); + temp = quatMul(rQuat, vQuat); + temp1 = quatConj(rQuat); + retVec = quatMul(temp, temp1); + + rows = retVec->height; + cols = retVec->width; + + ret = fSetArray(rows, 3, 0); + + for(i=0; i +#include +#include "localization.h" + +F2D* readSensorData(I2D* index, F2D* fid, I2D* type, I2D* eof) +{ + F2D *retData; + int rows, i, j, k; + int atype=-1, aindex; + + aindex = asubsref(index, 0); + + asubsref(index,0) = asubsref(index,0) + 1; + rows = fid->height; + asubsref(type,0) = 0; + retData = fSetArray(1, 8, 0); + + if( asubsref(index,0) > (rows-1) ) + asubsref(eof,0) = 1; + else + { + if( asubsref(index,0) == rows) + asubsref(eof,0) = 1; + else + asubsref(eof,0) = 0; + + k = asubsref(index,0); + atype = subsref(fid, k, 1); + if( (atype == 1) || (atype == 2) || (atype == 3) ) + { + for(i=0; i<3; i++) + { + asubsref(retData,i) = subsref(fid,k,(i+2)); + } + } + if( atype == 4 ) + { + for(i=0; i<3; i++) + { + asubsref(retData,i) = subsref(fid,k,(i+2)); + } + for(i=3; i<8; i++) + { + asubsref(retData,i) = subsref(fid,k+1,(i-3)); + } + aindex = aindex + 1; + } + aindex = aindex + 1; + } + + asubsref(index,0) = aindex; + asubsref(type, 0) = atype; + + return retData; +} + + + + diff --git a/SD-VBS/benchmarks/localization/src/c/script_localization.c b/SD-VBS/benchmarks/localization/src/c/script_localization.c new file mode 100644 index 0000000..3392320 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/script_localization.c @@ -0,0 +1,534 @@ +/******************************** +Author: Sravanthi Kota Venkata +********************************/ + +#include +#include +#include +#include "localization.h" +#include "extra.h" +#define LOCOLIZATION_MEM 1<<24 +int main(int argc, char* argv[]) +{ + SET_UP + mallopt(M_TOP_PAD, LOCOLIZATION_MEM); + mallopt(M_MMAP_MAX, 0); + int n, i, j, k, icount=-1; + F2D* fid; + float gyroTimeInterval=0.01; + float acclTimeInterval=0.01; + + float STDDEV_GPSVel=0.5; + float STDDEV_ODOVel=0.1; + float STDDEV_ACCL=1; + float M_STDDEV_GYRO=0.1; + float M_STDDEV_POS=0.1; + float M_STDDEV_VEL=0.02; + + F2D *pos, *vel; + float pi = 3.1416; + F2D *eul1, *eul2, *quat; + F2D *sData, *gyro, *norm_gyro, *angleAlpha; + F2D *quatDelta, *Opos, *temp_STDDEV_GPSPos, *w; + F2D *qConj, *orgWorld, *accl, *gtemp; + F2D *gravity, *t1; + I2D *tStamp, *sType, *isEOF; + I2D *index; + int rows, cols; + F2D *resultMat; + F2D *STDDEV_GPSPos; + F2D *ones, *randW; + + char im1[100]; + + printf("Input txt File: "); + scanf("%s", im1); + + fid = readFile(im1); + + n = 1000; + + #ifdef test + n = 3; + gyroTimeInterval = 0.1; + acclTimeInterval = 0.1; + M_STDDEV_VEL = 0.2; + #endif + #ifdef sim_fast + n = 3; + #endif + #ifdef sim + n = 10; + #endif + #ifdef sqcif + n = 800; + #endif + #ifdef qcif + n = 500; + #endif + #ifdef vga + n = 2000; + #endif + #ifdef wuxga + n = 3000; + #endif + + resultMat = fSetArray(3,fid->height, 0); + + pos = fSetArray(n, 3, 0); + vel = fSetArray(n, 3, 0); + ones = fSetArray(n,1,1); + + + F2D *randn; + + randn = randWrapper(n,3); + printf("start\n"); + for_each_job{ + fResetArray(pos,n, 3, 0); + fResetArray(vel,n, 3, 0); + fResetArray(ones,n,1,1); + + { + int j; + for(i=0; iheight, gyro->width); + t1 = fDeepCopy(randW); + + for(i=0; i<(n*3); i++) + { + asubsref(t1,i) = asubsref(randW,i) * M_STDDEV_GYRO; + asubsref(gyro, i) += asubsref(t1,i); + asubsref(abc, i) = pow(asubsref(gyro, i), 2); + } + fFreeHandle(t1); + abcd = fSum2(abc, 2); + + norm_gyro = fMallocHandle(abcd->height,abcd->width); + angleAlpha = fMallocHandle(abcd->height, abcd->width); + + for(i=0; i<(abcd->height*abcd->width); i++) + { + asubsref(norm_gyro, i) = sqrt(asubsref(abcd,i)); + asubsref(angleAlpha,i) = asubsref(norm_gyro,i) * gyroTimeInterval; + } + + qD_r += angleAlpha->height + gyro->height; + qD_c += angleAlpha->width + 3; + + fFreeHandle(t); + fFreeHandle(abcd); + + cosA = fSetArray(angleAlpha->height, angleAlpha->width, 0); + sinA = fSetArray(angleAlpha->height, angleAlpha->width, 0); + + for(i=0; i<(cosA->height*cosA->width); i++) + asubsref(cosA,i) = cos( asubsref(angleAlpha,i) /2 ); + + for(i=0; i<(sinA->height*sinA->width); i++) + asubsref(sinA,i) = sin( asubsref(angleAlpha,i) /2 ); + + + fFreeHandle(abc); + abc = fSetArray(1,3,1); + t1 = fMtimes(norm_gyro, abc); + t = ffDivide(gyro, t1); + fFreeHandle(t1); + + abcd = fMtimes(sinA, abc); + t1 = fTimes(t, abcd); + quatDelta = fHorzcat(cosA, t1); + + fFreeHandle(abcd); + fFreeHandle(t); + fFreeHandle(t1); + fFreeHandle(abc); + + t = quatMul(quat, quatDelta); + fFreeHandle(quat); + fFreeHandle(quatDelta); + quat = fDeepCopy(t); + + fFreeHandle(t); + fFreeHandle(norm_gyro); + fFreeHandle(gyro); + fFreeHandle(angleAlpha); + fFreeHandle(cosA); + fFreeHandle(sinA); + + } + } + + if( asubsref(sType,0) ==4) + { + //Observation + + float tempSum=0; + F2D *Ovel; + float OvelNorm; + int i; + + + asubsref(STDDEV_GPSPos, 0) = asubsref(sData, 6); + asubsref(STDDEV_GPSPos, 4) = asubsref(sData,7); + asubsref(STDDEV_GPSPos, 8) = 15; + + Opos = fDeepCopyRange(sData, 0, 1, 0, 3); + + //Initialize + + for(i=0; i<(pos->height*pos->width); i++) + tempSum += asubsref(pos,i); + + if(tempSum == 0) + { + F2D *t, *t1; + t = fMtimes( randW, STDDEV_GPSPos); + t1 = fMtimes(ones, Opos); + + for(i=0; i<(pos->height*pos->width); i++) + asubsref(pos,i) = asubsref(t,i) + asubsref(t1,i); + + fFreeHandle(t); + fFreeHandle(t1); + } + else + { + int rows, cols; + int mnrows, mncols; + + rows = STDDEV_GPSPos->height; + cols = STDDEV_GPSPos->width; + + temp_STDDEV_GPSPos = fSetArray(rows,cols,1); + for( mnrows=0; mnrows0.5) + { + F2D *t; + t = fDeepCopy(Ovel); + fFreeHandle(Ovel); + /* This is a double precision division */ + Ovel = fDivide(t, OvelNorm); + qConj = quatConj(quat); + fFreeHandle(t); + + { + t = fSetArray(1,3,0); + subsref(t,0,0) = 1; + orgWorld = quatRot(t, qConj); + fFreeHandle(t); + fFreeHandle(qConj); + t = fSetArray(3,3,0); + asubsref(t,0) = 1; + asubsref(t,4) = 1; + asubsref(t,8) = 1; + + { + int i; + for(i=0; i<(t->height*t->width); i++) + asubsref(t, i) = asubsref(t,i)/STDDEV_GPSVel; + w = mcl( orgWorld, Ovel, t); + generateSample(w, quat, vel, pos); + } + + fFreeHandle(t); + fFreeHandle(w); + fFreeHandle(orgWorld); + } + } + fFreeHandle(Ovel); + } + + if( asubsref(sType,0) ==1) + { + //Observation + F2D *Ovel; + F2D *t, *t1, *t2; + float valVel; + + t = fSetArray(vel->height, 1, 0); + + for(i=0; iheight; i++) + { + subsref(t,i,0) = sqrt(pow(subsref(vel,i,0),2) + pow(subsref(vel,i,1),2) + pow(subsref(vel,i,2),2)); + } + + Ovel = fSetArray(1, 1, asubsref(sData,0)); + valVel = 1.0/STDDEV_ODOVel; + + t1 = fSetArray(1,1,(1.0/STDDEV_ODOVel)); + w = mcl (t, Ovel, t1); + generateSample(w, quat, vel, pos); + + + fFreeHandle(w); + fFreeHandle(t); + fFreeHandle(t1); + fFreeHandle(Ovel); + } + + if( asubsref(sType,0) ==3) + { + //Observation + F2D *t; + t = fSetArray(1, 3, 0); + asubsref(t,2) = -9.8; + + accl = fDeepCopyRange(sData, 0, 1, 0, 3); + gtemp = fMtimes( ones, t); + gravity = quatRot(gtemp, quat); + + fFreeHandle(gtemp); + fFreeHandle(t); + t = fSetArray(3,3,0); + asubsref(t,0) = 1; + asubsref(t,4) = 1; + asubsref(t,8) = 1; + + { + int i; + for(i=0; i<(t->height*t->width); i++) + asubsref(t,i) = asubsref(t,i)/STDDEV_ACCL; + w = mcl( gravity, accl, t); + } + + generateSample(w, quat, vel, pos); + fFreeHandle(t); + //Motion model + t = fMtimes(ones, accl); + fFreeHandle(accl); + + accl = fMinus(t, gravity); + + fFreeHandle(w); + fFreeHandle(gravity); + fFreeHandle(t); + + { + //pos=pos+quatRot(vel,quatConj(quat))*acclTimeInterval+1/2*quatRot(accl,quatConj(quat))*acclTimeInterval^2+randn(n,3)*M_STDDEV_POS; + + F2D *s, *is; + int i; + is = quatConj(quat); + s = quatRot(vel, is); + fFreeHandle(is); + + for(i=0; i<(s->height*s->width); i++) + { + asubsref(s,i) = asubsref(s,i)*acclTimeInterval; //+(1/2); + } + is = fPlus(pos, s); + fFreeHandle(pos); + pos = fDeepCopy(is); + fFreeHandle(is); + fFreeHandle(s); + + /** pos_ above stores: pos+quatRot(vel,quatConj(quat))*acclTimeInterval **/ + + is = quatConj(quat); + s = quatRot(accl, is); + t = fDeepCopy(s); + for(i=0; i<(s->height*s->width); i++) + { + asubsref(t,i) = 1/2*asubsref(s,i)*acclTimeInterval*acclTimeInterval; + } + + /** t_ above stores: 1/2*quatRot(accl,quatCong(quat))*acclTimeInterval^2 **/ + + fFreeHandle(s); + fFreeHandle(is); + s = randnWrapper(n,3); + + for(i=0; i<(s->height*s->width); i++) + { + asubsref(s,i) = asubsref(s,i) * M_STDDEV_POS; + } + + /** s_ above stores: randn(n,3)*M_STDDEV_POS **/ + + is = fPlus(pos, t); + fFreeHandle(pos); + pos = fPlus(is, s); + + fFreeHandle(s); + fFreeHandle(t); + fFreeHandle(is); + + // vel=vel+accl*acclTimeInterval+randn(n,3)*M_STDDEV_VEL; %?? + + t = fDeepCopy(accl); + for(i=0; i<(accl->height*accl->width); i++) + asubsref(t,i) = asubsref(accl,i) * acclTimeInterval; + + is = fPlus(vel, t); + + fFreeHandle(accl); + fFreeHandle(t); + s = randnWrapper(n,3); + for(i=0; i<(s->height*s->width); i++) + { + asubsref(s,i) = asubsref(s,i) * M_STDDEV_VEL; + } + + fFreeHandle(vel); + vel = fPlus(is, s); + fFreeHandle(is); + fFreeHandle(s); + } + + } + + // Self check + { + F2D* temp; + float quatOut=0, velOut=0, posOut=0; + int i; + + for(i=0; i<(quat->height*quat->width); i++) + quatOut += asubsref(quat, i); + + for(i=0; i<(vel->height*vel->width); i++) + velOut += asubsref(vel, i); + + for(i=0; i<(pos->height*pos->width); i++) + posOut += asubsref(pos, i); + + subsref(resultMat,0,icount) = quatOut; + subsref(resultMat,1,icount) = velOut; + subsref(resultMat,2,icount) = posOut; + } + + fFreeHandle(sData); + + + if (asubsref(isEOF,0) == 1) + break; + } + } + printf("end..\n"); + +#ifdef CHECK + + // Self checking - use expected.txt from data directory + { + int ret=0; + float tol = 2.0; +#ifdef GENERATE_OUTPUT + fWriteMatrix(resultMat, argv[1]); +#endif + ret = fSelfCheck(resultMat, "./expected_C.txt", tol); + if (ret == -1) + printf("Error in Localization\n"); + } + // Self checking done +#endif + + + fFreeHandle(STDDEV_GPSPos); + iFreeHandle(index); + iFreeHandle(sType); + iFreeHandle(isEOF); + fFreeHandle(fid); + fFreeHandle(resultMat); + fFreeHandle(pos); + fFreeHandle(vel); + fFreeHandle(quat); + fFreeHandle(ones); + fFreeHandle(randW); + fFreeHandle(randn); + WRITE_TO_FILE + return 0; + +} + + + diff --git a/SD-VBS/benchmarks/localization/src/c/weightedSample.c b/SD-VBS/benchmarks/localization/src/c/weightedSample.c new file mode 100644 index 0000000..28099e8 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/c/weightedSample.c @@ -0,0 +1,37 @@ +/******************************** +Author: Sravanthi Kota Venkata +********************************/ + +#include +#include +#include +#include "localization.h" + +I2D* weightedSample(F2D* w) +{ + I2D *bin; + F2D *seed; + int n, i, j; + + n = w->height; + seed = randWrapper(n, 1); + bin = iSetArray(n, 1, 0); + + for(i=0; i 0) + asubsref(bin,j) = asubsref(bin,j) + 1; + } + for(j=0; j Date: Thu, 22 Oct 2020 03:17:31 -0400 Subject: Make SD-VBS compatible with run_bench.sh and cleanup SD-VBS: - Run silently - Fix some whitespace errors - Don't duplicate extra.h - Auto-detect if building with LITMUS-RT - Disable result checking - Add helper symlinks Misc: - Remove unused code from libextra - Set some missing rt_param fields in libextra - Disable CSV info dump from computeSMTslowdown.sh - Widen scope of .gitignore on .txt files - Include list of 2MB DIS pair benchmarks and inputs --- .../localization/src/c/script_localization.c | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) (limited to 'SD-VBS/benchmarks/localization/src') diff --git a/SD-VBS/benchmarks/localization/src/c/script_localization.c b/SD-VBS/benchmarks/localization/src/c/script_localization.c index 3392320..ed37132 100644 --- a/SD-VBS/benchmarks/localization/src/c/script_localization.c +++ b/SD-VBS/benchmarks/localization/src/c/script_localization.c @@ -41,7 +41,6 @@ int main(int argc, char* argv[]) char im1[100]; - printf("Input txt File: "); scanf("%s", im1); fid = readFile(im1); @@ -83,11 +82,10 @@ int main(int argc, char* argv[]) F2D *randn; randn = randWrapper(n,3); - printf("start\n"); - for_each_job{ - fResetArray(pos,n, 3, 0); - fResetArray(vel,n, 3, 0); - fResetArray(ones,n,1,1); + for_each_job { + fResetArray(pos,n, 3, 0); + fResetArray(vel,n, 3, 0); + fResetArray(ones,n,1,1); { int j; @@ -95,7 +93,7 @@ int main(int argc, char* argv[]) for(j=0; j<3; j++) subsref(vel, i, j) += subsref(randn,i,j) * STDDEV_ODOVel; - } + } @@ -486,17 +484,15 @@ int main(int argc, char* argv[]) subsref(resultMat,1,icount) = velOut; subsref(resultMat,2,icount) = posOut; } - + fFreeHandle(sData); - - + if (asubsref(isEOF,0) == 1) break; } } - printf("end..\n"); -#ifdef CHECK +#ifdef CHECK // Self checking - use expected.txt from data directory { -- cgit v1.2.2