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; }