1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
|
/********************************
Author: Sravanthi Kota Venkata
********************************/
#include <stdio.h>
#include <stdlib.h>
#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; i<rows; i++)
{
index = asubsref(sampleXId, i) - 1;
for(j=0; j<quat->width; j++)
{
subsref(retQuat,i,j) = subsref(quat,index,j);
}
}
for(i=0; i<rows; i++)
{
index = asubsref(sampleXId, i) - 1;
for(j=0; j<vel->width; j++)
{
subsref(retVel,i,j) = subsref(vel,index,j);
}
}
for(i=0; i<rows; i++)
{
index = asubsref(sampleXId, i) - 1;
for(j=0; j<pos->width; j++)
{
subsref(retPos,i,j) = subsref(pos,index,j);
}
}
for(i=0; i<quat->height; i++)
{
for(j=0; j<quat->width; j++)
{
subsref(quat,i,j) = subsref(retQuat,i,j);
}
}
for(i=0; i<vel->height; i++)
{
for(j=0; j<vel->width; j++)
{
subsref(vel,i,j) = subsref(retVel,i,j);
}
}
for(i=0; i<pos->height; i++)
{
for(j=0; j<pos->width; j++)
{
subsref(pos,i,j) = subsref(retPos,i,j);
}
}
fFreeHandle(retQuat);
fFreeHandle(retVel);
fFreeHandle(retPos);
iFreeHandle(sampleXId);
return;
}
|
