void sbsRobSys::linearScan(double v1, double v2, double v3, double v4, double v5, double v6,
double x, double y, double z, int noOfScan)
{
moveJointTo(v1, v2, v3, v4, v5, v6);
sbsPoint3D original, target;
target.setXYZ(x,y,z);
updateTCP();
original = m_robot.m_tcp.o;
linearScan(original, target, noOfScan);
}
void sbsRobSys::multipleLinearScan(char* filename, int noOfScan)
{
int count = 0;
double v[6];
double x, y, z;
sbsPoint3D original;
sbsPoint3D target;
char savefile[256];
FILE* f = NULL;
f = fopen(filename,"r");
if (f == NULL)
{
cout << "Error opening file\n";
return;
}
while(fscanf (f, "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", &v[0], &v[1], &v[2], &v[3], &v[4], &v[5], &x, &y, &z) != EOF)
{
count++;
clearPolygons();
m_robot.rotateJointToTarget(v[0], v[1], v[2], v[3], v[4], v[5]);
m_robot.updateTCP();
original = m_robot.m_tcp.o;
target.setXYZ(x, y, z);
linearScan(original, target, noOfScan);
sprintf(savefile,"position_%d_%d.csv", count, noOfScan);
savePolygons(savefile, NULL);
}
fclose(f);
}
/*
* Remove "obj" from "pRef". We extract the table offset bits from "iref"
* and zap the corresponding entry, leaving a hole if it's not at the top.
*
* If the entry is not between the current top index and the bottom index
* specified by the cookie, we don't remove anything. This is the behavior
* required by JNI's DeleteLocalRef function.
*
* Note this is NOT called when a local frame is popped. This is only used
* for explicit single removals.
*
* Returns "false" if nothing was removed.
*/
bool IndirectRefTable::remove(u4 cookie, IndirectRef iref)
{
IRTSegmentState prevState;
prevState.all = cookie;
int topIndex = segmentState.parts.topIndex;
int bottomIndex = prevState.parts.topIndex;
assert(table_ != NULL);
assert(alloc_entries_ <= max_entries_);
assert(segmentState.parts.numHoles >= prevState.parts.numHoles);
int idx = extractIndex(iref);
bool workAroundAppJniBugs = false;
if (indirectRefKind(iref) == kIndirectKindInvalid && gDvmJni.workAroundAppJniBugs) {
idx = linearScan(iref, bottomIndex, topIndex, table_);
workAroundAppJniBugs = true;
if (idx == -1) {
LOGW("trying to work around app JNI bugs, but didn't find %p in table!", iref);
return false;
}
}
if (idx < bottomIndex) {
/* wrong segment */
LOGV("Attempt to remove index outside index area (%d vs %d-%d)",
idx, bottomIndex, topIndex);
return false;
}
if (idx >= topIndex) {
/* bad -- stale reference? */
LOGD("Attempt to remove invalid index %d (bottom=%d top=%d)",
idx, bottomIndex, topIndex);
return false;
}
if (idx == topIndex-1) {
// Top-most entry. Scan up and consume holes.
if (workAroundAppJniBugs == false && !checkEntry("remove", iref, idx)) {
return false;
}
table_[idx] = NULL;
int numHoles = segmentState.parts.numHoles - prevState.parts.numHoles;
if (numHoles != 0) {
while (--topIndex > bottomIndex && numHoles != 0) {
LOGV("+++ checking for hole at %d (cookie=0x%08x) val=%p",
topIndex-1, cookie, table_[topIndex-1]);
if (table_[topIndex-1] != NULL) {
break;
}
LOGV("+++ ate hole at %d", topIndex-1);
numHoles--;
}
segmentState.parts.numHoles = numHoles + prevState.parts.numHoles;
segmentState.parts.topIndex = topIndex;
} else {
segmentState.parts.topIndex = topIndex-1;
LOGV("+++ ate last entry %d", topIndex-1);
}
} else {
/*
* Not the top-most entry. This creates a hole. We NULL out the
* entry to prevent somebody from deleting it twice and screwing up
* the hole count.
*/
if (table_[idx] == NULL) {
LOGV("--- WEIRD: removing null entry %d", idx);
return false;
}
if (workAroundAppJniBugs == false && !checkEntry("remove", iref, idx)) {
return false;
}
table_[idx] = NULL;
segmentState.parts.numHoles++;
LOGV("+++ left hole at %d, holes=%d", idx, segmentState.parts.numHoles);
}
return true;
}
bool IndirectRefTable::contains(IndirectRef iref) const {
return linearScan(iref, 0, segmentState.parts.topIndex, table_) != -1;
}