static int flt_tile_importflt(flt_tile_t* self, const char* fname)
{
assert(self);
assert(fname);
LOGD("debug fname=%s", fname);
FILE* f = fopen(fname, "r");
if(f == NULL)
{
// skip silently
return 0;
}
size_t size = self->nrows*self->ncols*sizeof(short);
self->height = (short*) malloc(size);
if(self->height == NULL)
{
LOGE("malloc failed");
goto fail_height;
}
size_t rsize = self->ncols*sizeof(float);
float* rdata = (float*) malloc(rsize);
if(rdata == NULL)
{
LOGE("malloc failed");
goto fail_rdata;
}
int row;
for(row = 0; row < self->nrows; ++row)
{
size_t left = rsize;
unsigned char* data = (unsigned char*) rdata;
while(left > 0)
{
size_t bytes = fread(data, sizeof(unsigned char), left, f);
if(bytes == 0)
{
LOGE("fread failed");
goto fail_read;
}
left -= bytes;
data += bytes;
}
// need to swap byte order for big endian
data = (unsigned char*) rdata;
if(self->byteorder == FLT_MSBFIRST)
{
int i;
for(i = 0; i < self->ncols; ++i)
{
unsigned char d0 = data[4*i + 0];
unsigned char d1 = data[4*i + 1];
unsigned char d2 = data[4*i + 2];
unsigned char d3 = data[4*i + 3];
data[4*i + 0] = d3;
data[4*i + 1] = d2;
data[4*i + 2] = d1;
data[4*i + 3] = d0;
// convert data to feet
float* height = (float*) &data[4*i];
self->height[row*self->ncols + i] = (short) (meters2feet(*height) + 0.5f);
}
}
else
{
int i;
for(i = 0; i < self->ncols; ++i)
{
// convert data to feet
float* height = (float*) &data[4*i];
self->height[row*self->ncols + i] = (short) (meters2feet(*height) + 0.5f);
}
}
}
free(rdata);
fclose(f);
return 1;
// failure
fail_read:
free(rdata);
rdata = NULL;
fail_rdata:
free(self->height);
self->height = NULL;
fail_height:
fclose(f);
return 0;
}
void velocityNav::update()
{
//stopWatch.stop();
//stopWatch.start();
vpr::Interval cur_time = mNavWand->getTimeStamp();
vpr::Interval diff_time(cur_time-mLastTimeStamp);
mTimeDelta = diff_time.secf();
/* Cluster Sync Debug code
std::cout << "Delta: " << diff_time.getBaseVal() << std::endl;
std::cout << "Current: " << cur_time.getBaseVal() << "Last: "
<< mLastTimeStamp.getBaseVal() << "\n" << std::endl;
*/
mLastTimeStamp = cur_time;
if ( mTimeDelta > 2.0f ) // If the time is greater than 2 seconds ( 1/2 fps)
{
vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
<< clrOutNORM(clrCYAN,"VelNav: timeInstant too large: ")
<< mTimeDelta << std::endl << vprDEBUG_FLUSH;
//stopWatch.stop(); // Get a REALLY small delta time
//stopWatch.start();
}
//vprDEBUG_BEGIN(vprDBG_ALL,0)
// << "VelNav: ----- Update ----\n" << vprDEBUG_FLUSH;
//vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
// << "VelNav: timeInstant: " << mTimeDelta << std::endl
// << vprDEBUG_FLUSH;
// If we are not supposed to be active, then don't run
if ( ! this->isActive() )
{
return;
}
//////////////////////////////////
// do navigations...
//////////////////////////////////
// Define axes, in Juggler/OpenGL coordinate system (right handed)
gmtl::Vec3f trackerZaxis(0.0f, 0.0f, 1.0f);
gmtl::Vec3f trackerXaxis(1.0f, 0.0f, 0.0f);
gmtl::Vec3f trackerYaxis(0.0f, 1.0f, 0.0f);
// 9.8 m/s (METERS)
const gmtl::Vec3f gravity_meters_per_second(0.0f, -9.8f, 0.0f);
// to be set by switch (mUnits == METERS)
gmtl::Vec3f gravity(0.0f, -9.8f, 0.0f);
switch (mUnits)
{
case FEET:
meters2feet(gravity_meters_per_second, gravity);
break;
default:
case METERS:
gravity = gravity_meters_per_second;
break;
}
if ((mAllowRot) && (mRotating))
{
// Interpolates the rotation, and updates mCurPos matrix
this->scaled_rotate(mRotationalAcceleration);
}
if (mMode == DRIVE)
{
// get the axes of the tracking/pointing device
// NOTE: constrain to the Y axis in GROUND mode (no flying/hopping or
// diving faster than gravity allows)
gmtl::Matrix44f constrainedToY;
//mRotationalAcceleration.constrainRotAxis(false, true, false,
// constrainedToY);
gmtl::EulerAngleXYZf euler(0.0f, gmtl::makeYRot(mRotationalAcceleration),
0.0f);
gmtl::setRot(constrainedToY, euler); // Only allow Yaw (rot y)
gmtl::xform(trackerZaxis, constrainedToY, trackerZaxis);
gmtl::xform(trackerXaxis, constrainedToY, trackerXaxis);
gmtl::xform(trackerYaxis, constrainedToY, trackerYaxis);
}
else if (mMode == FLY)
{
// get the axes of the tracking/pointing device
gmtl::xform(trackerZaxis, mRotationalAcceleration, trackerZaxis);
gmtl::xform(trackerXaxis, mRotationalAcceleration, trackerXaxis);
gmtl::xform(trackerYaxis, mRotationalAcceleration, trackerYaxis);
}
// this is used to accumulate velocity added by navigation
gmtl::Vec3f velocityAccumulator(0.0f, 0.0f, 0.0f);
if (mMode == DRIVE) // if DRIVING --> we have GRAVITY
{
// add the velocity this timeslice/frame by the acceleration from
// gravity.
velocityAccumulator += mVelocityFromGravityAccumulator;
//vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
// << "velNav: drive: gravAccum: "
// << mVelocityFromGravityAccumulator << vprDEBUG_FLUSH;
// recalculate the current downward velocity from gravity.
// this vector then is accumulated with the rest of the velocity vectors
// each frame.
//mVelocityFromGravityAccumulator += (gravity * mTimeDelta);
mVelocityFromGravityAccumulator += (gravity * mTimeDelta);
//vprDEBUG_CONT(vprDBG_ALL,0)
// << " new vel: " << velocityAccumulator
// << " new grav: " << mVelocityFromGravityAccumulator << std::endl
// << vprDEBUG_FLUSH;
}
if (mAllowTrans)
{
// add velocity with respect to the tracking/pointing device
velocityAccumulator += (trackerZaxis * mVelocity[2]); // forward/reverse |reletive to tracker
velocityAccumulator += (trackerXaxis * mVelocity[0]); // strafe |reletive to tracker
velocityAccumulator += (trackerYaxis * mVelocity[1]); // rise/dive |reletive to tracker
}
// Get rid of some velocity due to damping in the system
mVelocity -= (mVelocity * (1.0f - mDamping));
// navigation just calculated navigator's next velocity
// now convert accumulated velocity to distance traveled this frame (by
// cancelling out time)
// NOTE: this is not the final distance, since we still have to do
// collision correction.
gmtl::Vec3f distanceToMove = velocityAccumulator * mTimeDelta;
//vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
// << "velNav: distToMove = velAcum * instant: " << velocityAccumulator
// << " * " << mTimeDelta << std::endl << vprDEBUG_FLUSH;
// --- TRANSLATION and COLLISION DETECTION --- //
bool did_collide; // Did we collide with anything
// The total correction on the movement (in modelspace)
gmtl::Vec3f total_correction;
navTranslate(distanceToMove,did_collide,total_correction);
if(did_collide) // If we hit something, stop falling
{
//vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
// << "Did collide: Setting gravAccum to 0,0,0\n"
// << vprDEBUG_FLUSH;
mVelocityFromGravityAccumulator.set(0.0f, 0.0f, 0.0f);
}
//vprDEBUG_END(vprDBG_ALL,0) << "---------------------\n" << vprDEBUG_FLUSH;
}
