//Character should fall back to a safer position
Action* FallBack::run() {

	//flood the map of all the places we can move to within a long range
	vector<Tile*> fallbackPositions;
	PathFinding::floodMap(fallbackPositions, Tree->Character->CurrentTile, Tree->Character->Speed * _tilesDeep);

	for (int i = 0; i < (int)fallbackPositions.size(); i++) {
		
		//remove any possible fall back positions that are occupied by other characters
		if (fallbackPositions[i]->IsOccupied == true) {

			fallbackPositions.erase(fallbackPositions.begin() + i);
			i--;
		}
	}

	//get the assumed enemy infulence in the level
	vector<InfulenceData> enemyInfulence = Tree->CharTeam->getAssumedEnemyInfulencedTiles();

	//weight the tiles against the enemies infulence across them
	for (int i = 0; i < (int)fallbackPositions.size(); i++) {
		for (int j = 0; j < (int)enemyInfulence.size(); j++) {

			InfulenceData loopedInfulence = enemyInfulence[j];

			//check to see if the infulence is that of the looped fall back position
			if (fallbackPositions[i] == loopedInfulence.TheTile) {

				//increase the weighting
				fallbackPositions[i]->Weighting += loopedInfulence.Infulence;
				break;
			}
		}
	}

	vector<AICharacter*> visibleEnemies = Tree->CharTeam->getVisibleEnemies();

	//weight the possible positions by how far away they are from enemies
	for (int i = 0; i < (int)fallbackPositions.size(); i++) {

		//find the angle from the characters current position to the looped enemy
		Vector3 dir = fallbackPositions[i]->Position - Tree->Character->Position;

		//resolve vector into an angle, where 0 is facing up n degrees
		float angleToTile = ((atan2(dir.y, dir.x) * 180) / 3.1415926) + 90;

		for (int j = 0; j < (int)visibleEnemies.size(); j++) {

			float distance = visibleEnemies[j]->Position.distance(fallbackPositions[i]->Position);

			if (distance <= FALLBACK_ENEMY_TOO_CLOSE_CONST)
				fallbackPositions[i]->Weighting += distance * 10;

			else
				fallbackPositions[i]->Weighting -= visibleEnemies[j]->Position.distance(fallbackPositions[i]->Position) / 10;

			//find the angle from the characters current position to the looped enemy
			dir = visibleEnemies[j]->Position - Tree->Character->Position;

			//resolve vector into an angle, where 0 is facing up n degrees
			float angleToEnemy = ((atan2(dir.y, dir.x) * 180) / 3.1415926) + 90;

			//if the angle to the enemy is similair to the angle to the possible fallback position, then we are moving towards an enemy
			//and thus should be peanalised heavily
			if (Rotations::rotationsSimilair(angleToTile, angleToEnemy, 100) == true) {

				fallbackPositions[i]->Weighting += FALLBACK_FLANK_PENALTY_CONST;
			}
		}

		//now increase weighting by how far we guess we have to move
		int xCost = abs(Tree->Character->CurrentTile->X - fallbackPositions[i]->X);
		int yCost = abs(Tree->Character->CurrentTile->Y - fallbackPositions[i]->Y);

		//guess the distance to the end tile
		int Hcost = (xCost + yCost) * 10;
		fallbackPositions[i]->Weighting += Hcost * 10;
	}

	//loop through all of the possible points and find that with the lowest weighting
	Tile * bestTile = fallbackPositions[0];
	for (int i = 1; i < (int)fallbackPositions.size(); i++) {

		if (bestTile->Weighting > fallbackPositions[i]->Weighting) {
			bestTile = fallbackPositions[i];
			i = 1;
		}
	}

	//reset all of the tile's weighting for any later pathfinding
	for (int i = 0; i < (int)fallbackPositions.size(); i++) {
		fallbackPositions[i]->Weighting = 0;
	}

	Tree->log("Wants to fall back");
	return new MoveAction(Tree->Character, bestTile, 1);
}
Example #2
0
int PathPlanner::computeHeuristic(int xs, int ys, int xg, int yg) {
	return 4*std::min(abs(nodeMap[xs][ys].x -nodeMap[xg][yg].x),abs(nodeMap[xs][ys].y -nodeMap[xg][yg].y))+3*abs(abs(nodeMap[xs][ys].x -nodeMap[xg][yg].x)-abs(nodeMap[xs][ys].y -nodeMap[xg][yg].y));

}
Example #3
0
//cSurface* WINAPI DLLExport GetRunObjectSurface(LPRDATA rdPtr)
short WINAPI DLLExport DisplayRunObject(LPRDATA rdPtr)
{
	MaskChanged();

	if(rdPtr->dispTarget)
	{
		rdPtr->currentId = rdPtr->targetId;
		CurrentImg = TargetImg;
	}

	if(!CurrentImg)
	{
		rdPtr->rHo.hoImgWidth = 0;
		rdPtr->rHo.hoImgHeight = 0;
		return 0;
	}

	//Get scaled size of the surface
	int scaleW = CurrentImg->GetWidth()*abs(rdPtr->rc.rcScaleX);
	int scaleH = CurrentImg->GetHeight()*abs(rdPtr->rc.rcScaleY);

	//Object is too small
	if(scaleW <= 0 || scaleH <= 0)
	{
		rdPtr->rHo.hoImgWidth = 0;
		rdPtr->rHo.hoImgHeight = 0;
		rdPtr->rHo.hoImgXSpot = 0;
		rdPtr->rHo.hoImgYSpot = 0;
		return 0;
	}

	// Begin render process...
	LPRH rhPtr = rdPtr->rHo.hoAdRunHeader;
	LPSURFACE ps = WinGetSurface((int)rhPtr->rhIdEditWin);
	cSurface* renderImage = CurrentImg;

	// On-screen coords
	int screenX = rdPtr->rHo.hoX - rhPtr->rhWindowX;
	int screenY = rdPtr->rHo.hoY - rhPtr->rhWindowY;

	// Pure HWA blitting, yay!
#ifdef HWABETA

	if (rdPtr->isHWA)// && CurrentImg->GetType() >= ST_HWA_RTTEXTURE)
	{
		DWORD flags = 0;

		// Rotate quality 1
		if (rdPtr->rc.rcAngle && rdPtr->rs.rsFlags & RSFLAG_ROTATE_ANTIA)
			flags |= BLTF_ANTIA;

		// Scale quality 1
		if ((rdPtr->rc.rcScaleX != 1.0f || rdPtr->rc.rcScaleY != 1.0f) && rdPtr->rs.rsFlags & RSFLAG_SCALE_RESAMPLE)
			flags |= BLTF_ANTIA;

		// Hot spot (transform center)
		POINT point = {ImageS(rdPtr->currentId)->hotX, ImageS(rdPtr->currentId)->hotY};

		CurrentImg->BlitEx(*ps, screenX, screenY, 
			rdPtr->rc.rcScaleX, rdPtr->rc.rcScaleY, 0, 0,
			CurrentImg->GetWidth(), CurrentImg->GetHeight(), &point, rdPtr->rc.rcAngle,
			(rdPtr->rs.rsEffect & EFFECTFLAG_TRANSPARENT) ? BMODE_TRANSP : BMODE_OPAQUE,
			BlitOp(rdPtr->rs.rsEffect & EFFECT_MASK),
			rdPtr->rs.rsEffectParam, flags);
		return 0;
	}

#endif

	// Software blit: Offset draw position by transformed hotspot
	screenX -= rdPtr->rHo.hoImgXSpot;
	screenY -= rdPtr->rHo.hoImgYSpot;

	// Temporary surface for transformations
	cSurface temp;

	//Need scaling
	if(rdPtr->rc.rcScaleX != 1.0 || rdPtr->rc.rcScaleY != 1.0)
	{
		temp.Create(scaleW, scaleH, renderImage);
		renderImage->Stretch(temp, 0, 0, scaleW, scaleH, BMODE_OPAQUE, BOP_COPY, 0, 
			((rdPtr->rs.rsFlags&RSFLAG_SCALE_RESAMPLE)?STRF_RESAMPLE:0)|STRF_COPYALPHA);
	
		//Mirror for negative values.
		if(rdPtr->rc.rcScaleX < 0)
			temp.ReverseX();
		if(rdPtr->rc.rcScaleY < 0)
			temp.ReverseY();

		// For now, the scaled image is the 'final' one
		renderImage = &temp;
	}

	//Need rotation
	if(rdPtr->rc.rcAngle)
	{
		cSurface rotateBuffer;
		rotateBuffer.Clone(*renderImage);
		rotateBuffer.CreateRotatedSurface(temp, rdPtr->rc.rcAngle, 
			rdPtr->rs.rsFlags & RSFLAG_ROTATE_ANTIA, CurrentImg->GetTransparentColor(), TRUE);
		renderImage = &temp;
	}

	if (renderImage && renderImage->IsValid())
	{
		rdPtr->rHo.hoImgWidth = renderImage->GetWidth();
		rdPtr->rHo.hoImgHeight = renderImage->GetHeight();

		renderImage->Blit(
			*ps,
			screenX,
			screenY,
			0, 0,
			renderImage->GetWidth(),
			renderImage->GetHeight(),
			(rdPtr->rs.rsEffect & EFFECTFLAG_TRANSPARENT) ? BMODE_TRANSP : BMODE_OPAQUE,
			BlitOp(rdPtr->rs.rsEffect & EFFECT_MASK),
			rdPtr->rs.rsEffectParam
		);

		//Update window
		RECT rect;
		rect.left = screenX;
		rect.top = screenY;
		rect.right = screenX + renderImage->GetWidth();
		rect.bottom = screenY + renderImage->GetHeight();
		//ps->Rectangle(rect.left, rect.top, rect.right, rect.bottom, RED, 0, 0, TRUE);
		WinAddZone(rhPtr->rhIdEditWin, &rect);
	}

	return 0;
}
ULONG GetBitmapSize(LPCVOID pDib)
{
    ULONG nHeaderSize = GetBitmapHeaderSize(pDib);

    if (nHeaderSize == 0)
    {
        return 0;
    }

    // Start the calculation with the header size

    ULONG nDibSize = nHeaderSize;

    // is this an old style BITMAPCOREHEADER?

    if (nHeaderSize == sizeof(BITMAPCOREHEADER))
    {
        PBITMAPCOREHEADER pbmch = (PBITMAPCOREHEADER) pDib;

        // Add the color table size

        if (pbmch->bcBitCount <= 8)
        {
            nDibSize += sizeof(RGBTRIPLE) * (1i64 << pbmch->bcBitCount);
        }

        // Add the bitmap size

        ULONG nWidth = GetBitmapLineWidthInBytes(pbmch->bcWidth, pbmch->bcBitCount);

        nDibSize += nWidth * pbmch->bcHeight;
    }
    else
    {
        // this is at least a BITMAPINFOHEADER

        PBITMAPINFOHEADER pbmih = (PBITMAPINFOHEADER) pDib;

        // Add the color table size

        if (pbmih->biClrUsed != 0)
        {
            nDibSize += sizeof(RGBQUAD) * pbmih->biClrUsed;
        }
        else if (pbmih->biBitCount <= 8)
        {
            nDibSize += sizeof(RGBQUAD) * (1i64 << pbmih->biBitCount);
        }

        // Add the bitmap size

        if (pbmih->biSizeImage != 0)
        {
            nDibSize += pbmih->biSizeImage;
        }
        else
        {
            // biSizeImage must be specified for compressed bitmaps

            if (pbmih->biCompression != BI_RGB &&
                    pbmih->biCompression != BI_BITFIELDS)
            {
                return 0;
            }

            ULONG nWidth = GetBitmapLineWidthInBytes(pbmih->biWidth, pbmih->biBitCount);

            nDibSize += nWidth * abs(pbmih->biHeight);
        }

        // Consider special cases

        if (nHeaderSize == sizeof(BITMAPINFOHEADER))
        {
            // If this is a 16 or 32 bit bitmap and BI_BITFIELDS is used,
            // bmiColors member contains three DWORD color masks.
            // For V4 or V5 headers, this info is included the header

            if (pbmih->biCompression == BI_BITFIELDS)
            {
                nDibSize += 3 * sizeof(DWORD);
            }
        }
        else if (nHeaderSize >= sizeof(BITMAPV5HEADER))
        {
            // If this is a V5 header and an ICM profile is specified,
            // we need to consider the profile data size

            PBITMAPV5HEADER pbV5h = (PBITMAPV5HEADER) pDib;

            // if there is some padding before the profile data, add it

            if (pbV5h->bV5ProfileData > nDibSize)
            {
                nDibSize = pbV5h->bV5ProfileData;
            }

            // add the profile data size

            nDibSize += pbV5h->bV5ProfileSize;
        }
    }

    return nDibSize;
}
Example #5
0
void monster::die(game *g)
{
// Drop goodies
 int total_chance = 0, total_it_chance, cur_chance, selected_location,
     selected_item;
 bool animal_done = false;
 std::vector<items_location_and_chance> it = g->monitems[type->id];
 std::vector<itype_id> mapit;
 if (type->item_chance != 0 && it.size() == 0)
  debugmsg("Type %s has item_chance %d but no items assigned!",
           type->name.c_str(), type->item_chance);
 else {
  for (int i = 0; i < it.size(); i++)
   total_chance += it[i].chance;

  while (rng(0, 99) < abs(type->item_chance) && !animal_done) {
   cur_chance = rng(1, total_chance);
   selected_location = -1;
   while (cur_chance > 0) {
    selected_location++;
    cur_chance -= it[selected_location].chance;
   }
   total_it_chance = 0;
   mapit = g->mapitems[it[selected_location].loc];
   for (int i = 0; i < mapit.size(); i++)
    total_it_chance += g->itypes[mapit[i]]->rarity;
   cur_chance = rng(1, total_it_chance);
   selected_item = -1;
   while (cur_chance > 0) {
    selected_item++;
    cur_chance -= g->itypes[mapit[selected_item]]->rarity;
   }
   g->m.add_item(posx, posy, g->itypes[mapit[selected_item]], 0);
   if (type->item_chance < 0)
    animal_done = true;	// Only drop ONE item.
  }
 } // Done dropping items

// If we're a queen, make nearby groups of our type start to die out
 if (has_flag(MF_QUEEN)) {
  std::vector<mongroup*> groups = g->cur_om.monsters_at(g->levx, g->levy);
  for (int i = 0; i < groups.size(); i++) {
   moncat_id moncat_type = groups[i]->type;
   bool match = false;
   for (int j = 0; !match && j < g->moncats[moncat_type].size(); j++) {
    if (g->moncats[moncat_type][j] == type->id)
     match = true;
   }
   if (match)
    groups[i]->dying = true;
  }
// Do it for overmap above/below too
  overmap tmp;
  if (g->cur_om.posz == 0)
   tmp = overmap(g, g->cur_om.posx, g->cur_om.posy, -1);
  else
   tmp = overmap(g, g->cur_om.posx, g->cur_om.posy, 0);

  groups = tmp.monsters_at(g->levx, g->levy);
  for (int i = 0; i < groups.size(); i++) {
   moncat_id moncat_type = groups[i]->type;
   bool match = false;
   for (int j = 0; !match && j < g->moncats[moncat_type].size(); j++) {
    if (g->moncats[moncat_type][j] == type->id)
     match = true;
   }
   if (match)
    groups[i]->dying = true;
  }
 }
// If we're a mission monster, update the mission
 if (mission_id != -1) {
  mission_type *misstype = g->find_mission_type(mission_id);
  if (misstype->goal == MGOAL_FIND_MONSTER)
   g->fail_mission(mission_id);
  if (misstype->goal == MGOAL_KILL_MONSTER)
   g->mission_step_complete(mission_id, 1);
 }
// Also, perform our death function
 mdeath md;
 (md.*type->dies)(g, this);
// If our species fears seeing one of our own die, process that
 int anger_adjust = 0, morale_adjust = 0;
 for (int i = 0; i < type->anger.size(); i++) {
  if (type->anger[i] == MTRIG_FRIEND_DIED)
   anger_adjust += 15;
 }
 for (int i = 0; i < type->placate.size(); i++) {
  if (type->placate[i] == MTRIG_FRIEND_DIED)
   anger_adjust -= 15;
 }
 for (int i = 0; i < type->fear.size(); i++) {
  if (type->fear[i] == MTRIG_FRIEND_DIED)
   morale_adjust -= 15;
 }
 if (anger_adjust != 0 && morale_adjust != 0) {
  int light = g->light_level();
  for (int i = 0; i < g->z.size(); i++) {
   int t = 0;
   if (g->m.sees(g->z[i].posx, g->z[i].posy, posx, posy, light, t)) {
    g->z[i].morale += morale_adjust;
    g->z[i].anger += anger_adjust;
   }
  }
 }
}
Example #6
0
int tabulated_bonded_set_params(int bond_type, int tab_type, char * filename) 
{
  int i, token = 0, size;
  double dummr;
  FILE* fp;

  if(bond_type < 0)
    return 1;
  
  make_bond_type_exist(bond_type);
  
  fp = fopen( filename , "r");
  if ( !fp )
    return 3;
  
  /*Look for a line starting with # */
  while ( token != EOF) {
    token = fgetc(fp);
    if ( token == '#' ) { break; } // magic number for # symbol
  }
  if ( token == EOF ) { 
    fclose(fp);
    return 4;
  }

  /* set types */
  bonded_ia_params[bond_type].type       = BONDED_IA_TABULATED;
  bonded_ia_params[bond_type].p.tab.type = tab_type;

  /* set number of interaction partners */
  if(tab_type == TAB_BOND_LENGTH)   bonded_ia_params[bond_type].num = 1;
  if(tab_type == TAB_BOND_ANGLE)    bonded_ia_params[bond_type].num = 2;
  if(tab_type == TAB_BOND_DIHEDRAL) bonded_ia_params[bond_type].num = 3;

  /* copy filename */
  size = strlen(filename);
  bonded_ia_params[bond_type].p.tab.filename = (char*)malloc((size+1)*sizeof(char));
  strcpy(bonded_ia_params[bond_type].p.tab.filename,filename);

  /* read basic parameters from file */
  if (fscanf( fp , "%d %lf %lf", &size,
	      &bonded_ia_params[bond_type].p.tab.minval,
	      &bonded_ia_params[bond_type].p.tab.maxval) != 3) return 5;
  bonded_ia_params[bond_type].p.tab.npoints = size;

  /* Check interval for angle and dihedral potentials.  With adding
     ROUND_ERROR_PREC to the upper boundary we make sure, that during
     the calculation we do not leave the defined table!
  */
  if(tab_type == TAB_BOND_ANGLE ) {
    if( bonded_ia_params[bond_type].p.tab.minval != 0.0 || 
	abs(bonded_ia_params[bond_type].p.tab.maxval-PI) > 1e-5 ) {
      fclose(fp);
      return 6;
    }
    bonded_ia_params[bond_type].p.tab.maxval = PI+ROUND_ERROR_PREC;
  }
  /* check interval for angle and dihedral potentials */
  if(tab_type == TAB_BOND_DIHEDRAL ) {
    if( bonded_ia_params[bond_type].p.tab.minval != 0.0 || 
	abs(bonded_ia_params[bond_type].p.tab.maxval-(2*PI)) > 1e-5 ) {
      fclose(fp);
      return 6;
    }
    bonded_ia_params[bond_type].p.tab.maxval = (2*PI)+ROUND_ERROR_PREC;
  }

  /* calculate dependent parameters */
  bonded_ia_params[bond_type].p.tab.invstepsize = (double)(size-1)/(bonded_ia_params[bond_type].p.tab.maxval-bonded_ia_params[bond_type].p.tab.minval);

  /* allocate force and energy tables */
  bonded_ia_params[bond_type].p.tab.f = (double*)malloc(size*sizeof(double));
  bonded_ia_params[bond_type].p.tab.e = (double*)malloc(size*sizeof(double));

  /* Read in the new force and energy table data */
  for (i =0 ; i < size ; i++) {
    if (fscanf(fp,"%lf %lf %lf", &dummr,
	       &bonded_ia_params[bond_type].p.tab.f[i],
	       &bonded_ia_params[bond_type].p.tab.e[i]) != 3) return 5;
  }
  fclose(fp);

  mpi_bcast_ia_params(bond_type, -1); 

  return ES_OK;
}
Example #7
0
bool
AP_GPS_SBP::_attempt_state_update()
{

    // If we currently have heartbeats
    //    - NO FIX
    //
    // If we have a full update available, save it
    //
    uint32_t now = AP_HAL::millis();
    bool ret = false;

    if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) {

        state.status = AP_GPS::NO_FIX;
        Debug("No Heartbeats from Piksi! Driver Ready to Die!");

    } else if (last_pos_llh_rtk.tow == last_vel_ned.tow
            && abs((int32_t) (last_gps_time.tow - last_vel_ned.tow)) < 10000
            && abs((int32_t) (last_dops.tow - last_vel_ned.tow)) < 60000
            && last_vel_ned.tow > last_full_update_tow) {

        // Use the RTK position
        sbp_pos_llh_t *pos_llh = &last_pos_llh_rtk;

        // Update time state
        state.time_week         = last_gps_time.wn;
        state.time_week_ms      = last_vel_ned.tow;

        state.hdop              = last_dops.hdop;

        // Update velocity state
        state.velocity[0]       = (float)(last_vel_ned.n * 1.0e-3);
        state.velocity[1]       = (float)(last_vel_ned.e * 1.0e-3);
        state.velocity[2]       = (float)(last_vel_ned.d * 1.0e-3);
        state.have_vertical_velocity = true;

        float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
        state.ground_speed = safe_sqrt(ground_vector_sq);

        state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));

        // Update position state

        state.location.lat      = (int32_t) (pos_llh->lat * (double)1e7);
        state.location.lng      = (int32_t) (pos_llh->lon * (double)1e7);
        state.location.alt      = (int32_t) (pos_llh->height * 100);
        state.num_sats          = pos_llh->n_sats;

        if (pos_llh->flags == 0) {
            state.status = AP_GPS::GPS_OK_FIX_3D;
        } else if (pos_llh->flags == 2) {
            state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
        } else if (pos_llh->flags == 1) {
            state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
        }

        last_full_update_tow = last_vel_ned.tow;
        last_full_update_cpu_ms = now;
        state.rtk_iar_num_hypotheses = last_iar_num_hypotheses;

        logging_log_full_update();
        ret = true;

    } else if (now - last_full_update_cpu_ms > SBP_TIMEOUT_PVT) {

        //INVARIANT: If we currently have a fix, ONLY return true after a full update.

        state.status = AP_GPS::NO_FIX;
        ret = true;

    } else {

        //No timeouts yet, no data yet, nothing has happened.

    }

    return ret;

}
Example #8
0
int splitter_window_impl::override_size(unsigned & panel, int delta)
{
	//console::formatter() << "Overriding " << panel << " by " << delta;
	struct t_min_max_info
	{
		unsigned min_height;
		unsigned max_height;
		unsigned height;
		//unsigned caption_height;
	};

	unsigned count = m_panels.get_count();
	if (count)
	{
		save_sizes();
		if (panel + 1 < count)
		{
			unsigned n = 0;

			unsigned the_caption_height = g_get_caption_size();
			pfc::array_t<t_min_max_info> minmax;
			minmax.set_size(count);

			//minmax.fill(0);
			memset(minmax.get_ptr(), 0, minmax.get_size()*sizeof(t_min_max_info));

			for (n = 0; n<count; n++)
			{
				unsigned caption_height = m_panels[n]->m_show_caption && m_panels[n]->m_caption_orientation != get_orientation() ? the_caption_height : 0;
				unsigned min_height = m_panels[n]->m_hidden ? 0 : get_orientation() == vertical ? m_panels[n]->m_size_limits.min_height : m_panels[n]->m_size_limits.min_width;
				unsigned max_height = m_panels[n]->m_hidden ? 0 : get_orientation() == vertical ? m_panels[n]->m_size_limits.max_height : m_panels[n]->m_size_limits.max_width;

				if (min_height < (unsigned)(0 - caption_height)) min_height += caption_height;
				if (max_height < (unsigned)(0 - caption_height)) max_height += caption_height;

				if (get_orientation() == horizontal && m_panels[n]->m_show_toggle_area && !m_panels[n]->m_autohide)
				{
					if (max_height < unsigned(pfc_infinite) - 1)
						max_height++;
					if (min_height < unsigned(pfc_infinite) - 1)
						min_height++;
					caption_height++;
				}

				//minmax[n].caption_height = caption_height;
				minmax[n].min_height = min_height;
				minmax[n].max_height = max_height;
				minmax[n].height = m_panels[n]->m_hidden ? caption_height : m_panels[n]->m_size;
			}

			bool is_up = delta < 0;//new_height < m_panels[panel].height;
			bool is_down = delta > 0;//new_height > m_panels[panel].height;

			if (is_up /*&& !m_panels[panel].locked*/)
			{

				unsigned diff_abs = 0, diff_avail = abs(delta);

				unsigned n = panel + 1;
				while (n < count && diff_abs < diff_avail)
				{
					{
						unsigned height = minmax[n].height + (diff_avail - diff_abs);//(diff_avail-diff_abs > m_panels[n]->height ? 0 : m_panels[n]->height-(diff_avail-diff_abs));

						unsigned min_height = minmax[n].min_height;
						unsigned max_height = minmax[n].max_height;

						if (height < min_height)
						{
							height = min_height;
						}
						else if (height > max_height)
						{
							height = max_height;
						}

						diff_abs += height - minmax[n].height;
					}
					n++;
				}

				n = panel + 1;
				unsigned obtained = 0;
				while (n>0 && obtained < diff_abs)
				{
					n--;
					//					if (!m_panels[n]->locked)
					{
						unsigned height = (diff_abs - obtained > minmax[n].height ? 0 : minmax[n].height - (diff_abs - obtained));

						//unsigned caption_height = m_panels[n]->m_show_caption ? the_caption_height : 0;

						unsigned min_height = minmax[n].min_height;
						unsigned max_height = minmax[n].max_height;


						if (height < min_height)
						{
							height = min_height;
						}
						else if (height > max_height)
						{
							height = max_height;
						}

						obtained += minmax[n].height - height;
						minmax[n].height = height;
						if (!m_panels[n]->m_hidden) m_panels[n]->m_size = height;

					}
				}
				n = panel;
				unsigned obtained2 = obtained;

				while (n < count - 1 && obtained2)
				{
					n++;
					unsigned height = (minmax[n].height);

					unsigned min_height = minmax[n].min_height;
					unsigned max_height = minmax[n].max_height;

					height += obtained2;

					if (height < min_height)
					{
						height = min_height;
					}
					else if (height > max_height)
					{
						height = max_height;
					}

					obtained2 -= height - minmax[n].height;
					minmax[n].height = height;
					if (!m_panels[n]->m_hidden) m_panels[n]->m_size = height;
				}
				return (abs(delta) - obtained);


			}
			else if (is_down /*&& !m_panels[panel].locked*/)
			{
				unsigned diff_abs = 0, diff_avail = abs(delta);

				n = panel + 1;
				while (n >0 && diff_abs < diff_avail)
				{
					n--;
					{
						unsigned height = minmax[n].height + (diff_avail - diff_abs);//(diff_avail-diff_abs > m_panels[n]->height ? 0 : m_panels[n]->height-(diff_avail-diff_abs));
						//console::formatter() << "1: " << height << " " << minmax[n].height << " " << (diff_avail-diff_abs);

						unsigned min_height = minmax[n].min_height;
						unsigned max_height = minmax[n].max_height;

						if (height < min_height)
						{
							height = min_height;
						}
						else if (height > max_height)
						{
							height = max_height;
						}


						diff_abs += height - minmax[n].height;
					}
				}
				n = panel;
				unsigned obtained = 0;
				while (n < count - 1 && obtained < diff_abs)
				{
					n++;
					//				if (!m_panels[n]->locked)
					{
						unsigned height = (diff_abs - obtained > minmax[n].height ? 0 : minmax[n].height - (diff_abs - obtained));
						//console::formatter() << "2: " << height << " " << minmax[n].height << " " << (diff_abs-obtained);

						//unsigned caption_height = minmax[n].caption_height;
						unsigned min_height = minmax[n].min_height;
						unsigned max_height = minmax[n].max_height;


						if (height < min_height)
						{
							height = min_height;
						}
						else if (height > max_height)
						{
							height = max_height;
						}

						obtained += minmax[n].height - height;
						minmax[n].height = height;
						if (!m_panels[n]->m_hidden) m_panels[n]->m_size = height;

					}
				}
				n = panel + 1;
				unsigned obtained2 = obtained;
				while (n >0 && obtained2)
				{
					n--;
					unsigned height = (minmax[n].height);
					unsigned min_height = minmax[n].min_height;
					unsigned max_height = minmax[n].max_height;

					height += obtained2;

					if (height < min_height)
					{
						height = min_height;
					}
					else if (height > max_height)
					{
						height = max_height;
					}

					obtained2 -= height - minmax[n].height;

					minmax[n].height = height;

					if (!m_panels[n]->m_hidden) m_panels[n]->m_size = height;
				}
				//console::formatter() << "3: " << abs(delta) << " " << obtained;
				return 0 - (abs(delta) - obtained);

			}

		}
	}
	return 0;
}
Example #9
0
/********************************************************************************
							计算直线点描述子
********************************************************************************/
void  CDescriptor::ComputeSubRegionProjection(double* pSubRegionDes,double dMainArc,int nCenterR,int nCenterC)
{
	//取出9类小区域内的的梯度
	int nMainAngle = (int)(dMainArc*180/PI);
	double* pDataDx = new double[nMaxRegionNum*nEachRPixes];
	double* pDataDy = new double[nMaxRegionNum*nEachRPixes];
	for(int i=0; i<nMaxRegionNum; i++)
		for(int j=0; j<nEachRPixes; j++)
		{
			int k = i*nEachRPixes + j;
			int rr = LUTSubRegion[nMainAngle][k].nNo1 + nCenterR;
			int cc = LUTSubRegion[nMainAngle][k].nNo2 + nCenterC;
			int kk = rr*m_nWidth+cc;
			
			if(kk < 0 || kk > m_nTotolPixels-1)
			{
				continue;
			}
			pDataDx[k] = m_pDxImage[kk];
			pDataDy[k] = m_pDyImage[kk];
		}

	//主方向
	double dLineVx = cos(dMainArc);
	double dLineVy = sin(dMainArc);

	//计算每一类的四个分量
	for(int i=0; i< 4*nMaxRegionNum; i++)
	{
		pSubRegionDes[i] = 0;
	}
	for(int i=0; i<nMaxRegionNum*nEachRPixes; i++)
	{
		//梯度加权
		double dx = pDataDx[i]*LUTWeight[i];
		double dy = pDataDy[i]*LUTWeight[i];
		double IP = dx*dLineVx + dy*dLineVy;
		double EP = dx*dLineVy - dy*dLineVx;

		//查表获得最接近的2区域和相应的权值
		int nNo1 = LUTBiPos[i].nNo1;
		int nNo2 = LUTBiPos[i].nNo2;
		double dCoe1 = LUTBiPos[i].dCoe1;
		double dCoe2 = LUTBiPos[i].dCoe2;
		
		//累加到区域1上
		if(IP > 0)
		{
			pSubRegionDes[4*nNo1]	 = pSubRegionDes[4*nNo1] + IP*dCoe1;
		}
		else
		{
			pSubRegionDes[4*nNo1+2]	 = pSubRegionDes[4*nNo1+2] + abs(IP*dCoe1);
		}
		if(EP > 0)
		{
			pSubRegionDes[4*nNo1+1]	 = pSubRegionDes[4*nNo1+1] + EP*dCoe1;
		}
		else
		{
			pSubRegionDes[4*nNo1+3]	 = pSubRegionDes[4*nNo1+3] + abs(EP*dCoe1);
		}

		//累加到区域2上
		if(IP > 0)
		{
			pSubRegionDes[4*nNo2]	 = pSubRegionDes[4*nNo2] + IP*dCoe2;
		}
		else
		{
			pSubRegionDes[4*nNo2+2]	 = pSubRegionDes[4*nNo2+2] + abs(IP*dCoe2);
		}
		if(EP > 0)
		{
			pSubRegionDes[4*nNo2+1]	 = pSubRegionDes[4*nNo2+1] + EP*dCoe2;
		}
		else
		{
			pSubRegionDes[4*nNo2+3]	 = pSubRegionDes[4*nNo2+3] + abs(EP*dCoe2);
		}

	}
	/***********************************************************************/
	//释放内存
	wzhFreePointer(pDataDx);
	wzhFreePointer(pDataDy);
}
Example #10
0
File: dlasd5.c Project: vopl/sp
/* Subroutine */ int dlasd5_(integer *i__, doublereal *d__, doublereal *z__, 
	doublereal *delta, doublereal *rho, doublereal *dsigma, doublereal *
	work)
{
    /* System generated locals */
    doublereal d__1;

    /* Builtin functions */
    double sqrt(doublereal);

    /* Local variables */
    _THREAD_STATIC_ doublereal b, c__, w, del, tau, delsq;


/*  -- LAPACK auxiliary routine (version 3.1) --   
       Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..   
       November 2006   


    Purpose   
    =======   

    This subroutine computes the square root of the I-th eigenvalue   
    of a positive symmetric rank-one modification of a 2-by-2 diagonal   
    matrix   

               diag( D ) * diag( D ) +  RHO *  Z * transpose(Z) .   

    The diagonal entries in the array D are assumed to satisfy   

               0 <= D(i) < D(j)  for  i < j .   

    We also assume RHO > 0 and that the Euclidean norm of the vector   
    Z is one.   

    Arguments   
    =========   

    I      (input) INTEGER   
           The index of the eigenvalue to be computed.  I = 1 or I = 2.   

    D      (input) DOUBLE PRECISION array, dimension ( 2 )   
           The original eigenvalues.  We assume 0 <= D(1) < D(2).   

    Z      (input) DOUBLE PRECISION array, dimension ( 2 )   
           The components of the updating vector.   

    DELTA  (output) DOUBLE PRECISION array, dimension ( 2 )   
           Contains (D(j) - sigma_I) in its  j-th component.   
           The vector DELTA contains the information necessary   
           to construct the eigenvectors.   

    RHO    (input) DOUBLE PRECISION   
           The scalar in the symmetric updating formula.   

    DSIGMA (output) DOUBLE PRECISION   
           The computed sigma_I, the I-th updated eigenvalue.   

    WORK   (workspace) DOUBLE PRECISION array, dimension ( 2 )   
           WORK contains (D(j) + sigma_I) in its  j-th component.   

    Further Details   
    ===============   

    Based on contributions by   
       Ren-Cang Li, Computer Science Division, University of California   
       at Berkeley, USA   

    =====================================================================   


       Parameter adjustments */
    --work;
    --delta;
    --z__;
    --d__;

    /* Function Body */
    del = d__[2] - d__[1];
    delsq = del * (d__[2] + d__[1]);
    if (*i__ == 1) {
	w = *rho * 4. * (z__[2] * z__[2] / (d__[1] + d__[2] * 3.) - z__[1] * 
		z__[1] / (d__[1] * 3. + d__[2])) / del + 1.;
	if (w > 0.) {
	    b = delsq + *rho * (z__[1] * z__[1] + z__[2] * z__[2]);
	    c__ = *rho * z__[1] * z__[1] * delsq;

/*           B > ZERO, always   

             The following TAU is DSIGMA * DSIGMA - D( 1 ) * D( 1 ) */

	    tau = c__ * 2. / (b + sqrt((d__1 = b * b - c__ * 4., abs(d__1))));

/*           The following TAU is DSIGMA - D( 1 ) */

	    tau /= d__[1] + sqrt(d__[1] * d__[1] + tau);
	    *dsigma = d__[1] + tau;
	    delta[1] = -tau;
	    delta[2] = del - tau;
	    work[1] = d__[1] * 2. + tau;
	    work[2] = d__[1] + tau + d__[2];
/*           DELTA( 1 ) = -Z( 1 ) / TAU   
             DELTA( 2 ) = Z( 2 ) / ( DEL-TAU ) */
	} else {
	    b = -delsq + *rho * (z__[1] * z__[1] + z__[2] * z__[2]);
	    c__ = *rho * z__[2] * z__[2] * delsq;

/*           The following TAU is DSIGMA * DSIGMA - D( 2 ) * D( 2 ) */

	    if (b > 0.) {
		tau = c__ * -2. / (b + sqrt(b * b + c__ * 4.));
	    } else {
		tau = (b - sqrt(b * b + c__ * 4.)) / 2.;
	    }

/*           The following TAU is DSIGMA - D( 2 ) */

	    tau /= d__[2] + sqrt((d__1 = d__[2] * d__[2] + tau, abs(d__1)));
	    *dsigma = d__[2] + tau;
	    delta[1] = -(del + tau);
	    delta[2] = -tau;
	    work[1] = d__[1] + tau + d__[2];
	    work[2] = d__[2] * 2. + tau;
/*           DELTA( 1 ) = -Z( 1 ) / ( DEL+TAU )   
             DELTA( 2 ) = -Z( 2 ) / TAU */
	}
/*        TEMP = SQRT( DELTA( 1 )*DELTA( 1 )+DELTA( 2 )*DELTA( 2 ) )   
          DELTA( 1 ) = DELTA( 1 ) / TEMP   
          DELTA( 2 ) = DELTA( 2 ) / TEMP */
    } else {

/*        Now I=2 */

	b = -delsq + *rho * (z__[1] * z__[1] + z__[2] * z__[2]);
	c__ = *rho * z__[2] * z__[2] * delsq;

/*        The following TAU is DSIGMA * DSIGMA - D( 2 ) * D( 2 ) */

	if (b > 0.) {
	    tau = (b + sqrt(b * b + c__ * 4.)) / 2.;
	} else {
	    tau = c__ * 2. / (-b + sqrt(b * b + c__ * 4.));
	}

/*        The following TAU is DSIGMA - D( 2 ) */

	tau /= d__[2] + sqrt(d__[2] * d__[2] + tau);
	*dsigma = d__[2] + tau;
	delta[1] = -(del + tau);
	delta[2] = -tau;
	work[1] = d__[1] + tau + d__[2];
	work[2] = d__[2] * 2. + tau;
/*        DELTA( 1 ) = -Z( 1 ) / ( DEL+TAU )   
          DELTA( 2 ) = -Z( 2 ) / TAU   
          TEMP = SQRT( DELTA( 1 )*DELTA( 1 )+DELTA( 2 )*DELTA( 2 ) )   
          DELTA( 1 ) = DELTA( 1 ) / TEMP   
          DELTA( 2 ) = DELTA( 2 ) / TEMP */
    }
    return 0;

/*     End of DLASD5 */

} /* dlasd5_ */
Example #11
0
static void evaluateCommand(void)
{
    uint32_t i, j, tmp, junk;
#ifdef GPS
    uint8_t wp_no;
    int32_t lat = 0, lon = 0, alt = 0;
#endif
    if (osdSilence)
        osdDisableMSP();
    switch (currentPortState->cmdMSP) {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcData[i] = read16();
        headSerialReply(0);
        mspFrameRecieve();
        break;
    case MSP_SET_ACC_TRIM:
        cfg.angleTrim[PITCH] = read16();
        cfg.angleTrim[ROLL]  = read16();
        headSerialReply(0);
        break;
#ifdef GPS
    case MSP_SET_RAW_GPS:
        f.GPS_FIX = read8();
        GPS_numSat = read8();
        GPS_coord[LAT] = read32();
        GPS_coord[LON] = read32();
        GPS_altitude = read16();
        GPS_speed = read16();
        GPS_update |= 2;        // New data signalisation to GPS functions
        headSerialReply(0);
        break;
#endif
    case MSP_SET_PID:
        for (i = 0; i < PIDITEMS; i++) {
            cfg.P8[i] = read8();
            cfg.I8[i] = read8();
            cfg.D8[i] = read8();
        }
        headSerialReply(0);
        break;
    case MSP_SET_BOX:
        for (i = 0; i < numberBoxItems; i++)
            cfg.activate[availableBoxes[i]] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RC_TUNING:
        cfg.rcRate8 = read8();
        cfg.rcExpo8 = read8();
        cfg.rollPitchRate = read8();
        cfg.yawRate = read8();
        cfg.dynThrPID = read8();
        cfg.thrMid8 = read8();
        cfg.thrExpo8 = read8();
        headSerialReply(0);
        break;
    case MSP_SET_MISC:
        tmp = read16();
        // sanity check
        if (tmp < 1600 && tmp > 1400)
            mcfg.midrc = tmp;
        mcfg.minthrottle = read16();
        mcfg.maxthrottle = read16();
        mcfg.mincommand = read16();
        cfg.failsafe_throttle = read16();
        mcfg.gps_type = read8();
        mcfg.gps_baudrate = read8();
        mcfg.gps_ubx_sbas = read8();
        mcfg.multiwiicurrentoutput = read8();
        read16();
        cfg.mag_declination = read16() * 10;
        mcfg.vbatscale = read8();           // actual vbatscale as intended
        mcfg.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
        mcfg.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
        read8();                            // vbatlevel_crit (unused)
        headSerialReply(0);
        break;
    case MSP_SET_MOTOR:
        for (i = 0; i < 8; i++)
            motor_disarmed[i] = read16();
        headSerialReply(0);
        break;
    case MSP_SELECT_SETTING:
        if (!f.ARMED) {
            mcfg.current_profile = read8();
            if (mcfg.current_profile > 2)
                mcfg.current_profile = 0;
            // this writes new profile index and re-reads it
            writeEEPROM(0, false);
        }
        headSerialReply(0);
        break;
    case MSP_SET_HEAD:
        magHold = read16();
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(mcfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(CAP_PLATFORM_32BIT | CAP_BASEFLIGHT_CONFIG | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0));        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(11);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
        // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
        // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS.
        // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
        junk = 0;
        tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
                    f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
                    f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON |
                    rcOptions[BOXLEDMAX] << BOXLEDMAX |
                    rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
                    rcOptions[BOXVARIO] << BOXVARIO |
                    rcOptions[BOXCALIB] << BOXCALIB |
                    rcOptions[BOXGOV] << BOXGOV |
                    rcOptions[BOXOSD] << BOXOSD |
                    rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
                    f.ARMED << BOXARM;
        for (i = 0; i < numberBoxItems; i++) {
            int flag = (tmp & (1 << availableBoxes[i]));
            if (flag)
                junk |= 1 << i;
        }
        serialize32(junk);
        serialize8(mcfg.current_profile);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        // Retarded hack until multiwiidorks start using real units for sensor data
        if (acc_1G > 1024) {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i] / 8);
        } else {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i]);
        }
        for (i = 0; i < 3; i++)
            serialize16(gyroData[i]);
        for (i = 0; i < 3; i++)
            serialize16(magADC[i]);
        break;
    case MSP_SERVO:
        s_struct((uint8_t *)&servo, 16);
        break;
    case MSP_SERVO_CONF:
        headSerialReply(56);
        for (i = 0; i < MAX_SERVOS; i++) {
            serialize16(cfg.servoConf[i].min);
            serialize16(cfg.servoConf[i].max);
            serialize16(cfg.servoConf[i].middle);
            serialize8(cfg.servoConf[i].rate);
        }
        break;
    case MSP_SET_SERVO_CONF:
        headSerialReply(0);
        for (i = 0; i < MAX_SERVOS; i++) {
            cfg.servoConf[i].min = read16();
            cfg.servoConf[i].max = read16();
            cfg.servoConf[i].middle = read16();
            cfg.servoConf[i].rate = read8();
        }
        break;
    case MSP_MOTOR:
        s_struct((uint8_t *)motor, 16);
        break;
    case MSP_RC:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(rcData[i]);
        break;
#ifdef GPS
    case MSP_RAW_GPS:
        headSerialReply(16);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        serialize16(GPS_ground_course);
        break;
    case MSP_COMP_GPS:
        headSerialReply(5);
        serialize16(GPS_distanceToHome);
        serialize16(GPS_directionToHome);
        serialize8(GPS_update & 1);
        break;
#endif
    case MSP_ATTITUDE:
        headSerialReply(6);
        for (i = 0; i < 2; i++)
            serialize16(angle[i]);
        serialize16(heading);
        break;
    case MSP_ALTITUDE:
        headSerialReply(6);
        serialize32(EstAlt);
        serialize16(vario);
        break;
    case MSP_ANALOG:
        headSerialReply(7);
        serialize8((uint8_t)constrain(vbat, 0, 255));
        serialize16((uint16_t)constrain(mAhdrawn, 0, 0xFFFF)); // milliamphours drawn from battery
        serialize16(rssi);
        if (mcfg.multiwiicurrentoutput)
            serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
        else
            serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
        break;
    case MSP_RC_TUNING:
        headSerialReply(7);
        serialize8(cfg.rcRate8);
        serialize8(cfg.rcExpo8);
        serialize8(cfg.rollPitchRate);
        serialize8(cfg.yawRate);
        serialize8(cfg.dynThrPID);
        serialize8(cfg.thrMid8);
        serialize8(cfg.thrExpo8);
        break;
    case MSP_PID:
        headSerialReply(3 * PIDITEMS);
        for (i = 0; i < PIDITEMS; i++) {
            serialize8(cfg.P8[i]);
            serialize8(cfg.I8[i]);
            serialize8(cfg.D8[i]);
        }
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_BOX:
        headSerialReply(2 * numberBoxItems);
        for (i = 0; i < numberBoxItems; i++)
            serialize16(cfg.activate[availableBoxes[i]]);
        break;
    case MSP_BOXNAMES:
        // headSerialReply(sizeof(boxnames) - 1);
        serializeBoxNamesReply();
        break;
    case MSP_BOXIDS:
        headSerialReply(numberBoxItems);
        for (i = 0; i < numberBoxItems; i++) {
            for  (j = 0; j < CHECKBOXITEMS; j++) {
                if (boxes[j].permanentId == availableBoxes[i])
                    serialize8(boxes[j].permanentId);
            }
        }
        break;
    case MSP_MISC:
        headSerialReply(2 * 6 + 4 + 2 + 4);
        serialize16(mcfg.midrc);
        serialize16(mcfg.minthrottle);
        serialize16(mcfg.maxthrottle);
        serialize16(mcfg.mincommand);
        serialize16(cfg.failsafe_throttle);
        serialize8(mcfg.gps_type);
        serialize8(mcfg.gps_baudrate);
        serialize8(mcfg.gps_ubx_sbas);
        serialize8(mcfg.multiwiicurrentoutput);
        serialize16(0);
        serialize16(cfg.mag_declination / 10); // TODO check this shit
        serialize8(mcfg.vbatscale);
        serialize8(mcfg.vbatmincellvoltage);
        serialize8(mcfg.vbatmaxcellvoltage);
        serialize8(0);
        break;
    case MSP_MOTOR_PINS:
        headSerialReply(8);
        for (i = 0; i < 8; i++)
            serialize8(i + 1);
        break;
#ifdef GPS
    case MSP_WP:
        wp_no = read8();    // get the wp number
        headSerialReply(18);
        if (wp_no == 0) {
            lat = GPS_home[LAT];
            lon = GPS_home[LON];
        } else if (wp_no == 16) {
            lat = GPS_hold[LAT];
            lon = GPS_hold[LON];
        }
        serialize8(wp_no);
        serialize32(lat);
        serialize32(lon);
        serialize32(AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
        serialize16(0);                 // heading  will come here (deg)
        serialize16(0);                 // time to stay (ms) will come here
        serialize8(0);                  // nav flag will come here
        break;
    case MSP_SET_WP:
        wp_no = read8();    //get the wp number
        lat = read32();
        lon = read32();
        alt = read32();     // to set altitude (cm)
        read16();           // future: to set heading (deg)
        read16();           // future: to set time to stay (ms)
        read8();            // future: to set nav flag
        if (wp_no == 0) {
            GPS_home[LAT] = lat;
            GPS_home[LON] = lon;
            f.GPS_HOME_MODE = 0;        // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
            f.GPS_FIX_HOME = 1;
            if (alt != 0)
                AltHold = alt;          // temporary implementation to test feature with apps
        } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
            GPS_hold[LAT] = lat;
            GPS_hold[LON] = lon;
            if (alt != 0)
                AltHold = alt;          // temporary implementation to test feature with apps
            nav_mode = NAV_MODE_WP;
            GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
        }
        headSerialReply(0);
        break;
#endif /* GPS */
    case MSP_RESET_CONF:
        if (!f.ARMED)
            checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        if (!f.ARMED)
            calibratingA = CALIBRATING_ACC_CYCLES;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        if (!f.ARMED)
            f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        if (f.ARMED) {
            headSerialError(0);
        } else {
            writeEEPROM(0, true);
            headSerialReply(0);
        }
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        // make use of this crap, output some useful QA statistics
        debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
        for (i = 0; i < 4; i++)
            serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;

    // Additional commands that are not compatible with MultiWii
    case MSP_ACC_TRIM:
        headSerialReply(4);
        serialize16(cfg.angleTrim[PITCH]);
        serialize16(cfg.angleTrim[ROLL]);
        break;
    case MSP_UID:
        headSerialReply(12);
        serialize32(U_ID_0);
        serialize32(U_ID_1);
        serialize32(U_ID_2);
        break;
    case MSP_GPSSVINFO:
        headSerialReply(1 + (GPS_numCh * 4));
        serialize8(GPS_numCh);
           for (i = 0; i < GPS_numCh; i++){
               serialize8(GPS_svinfo_chn[i]);
               serialize8(GPS_svinfo_svid[i]);
               serialize8(GPS_svinfo_quality[i]);
               serialize8(GPS_svinfo_cno[i]);
            }
        break;

    case MSP_SET_CONFIG:
        headSerialReply(0);
        mcfg.mixerConfiguration = read8(); // multitype
        featureClearAll();
        featureSet(read32()); // features bitmap
        mcfg.serialrx_type = read8(); // serialrx_type
        mcfg.board_align_roll = read16(); // board_align_roll
        mcfg.board_align_pitch = read16(); // board_align_pitch
        mcfg.board_align_yaw = read16(); // board_align_yaw
        mcfg.currentscale = read16();
        mcfg.currentoffset = read16();
        /// ???
        break;
    case MSP_CONFIG:
        headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 4);
        serialize8(mcfg.mixerConfiguration);
        serialize32(featureMask());
        serialize8(mcfg.serialrx_type);
        serialize16(mcfg.board_align_roll);
        serialize16(mcfg.board_align_pitch);
        serialize16(mcfg.board_align_yaw);
        serialize16(mcfg.currentscale);
        serialize16(mcfg.currentoffset);
        /// ???
        break;

    case MSP_RCMAP:
        headSerialReply(MAX_INPUTS); // TODO fix this
        for (i = 0; i < MAX_INPUTS; i++)
            serialize8(mcfg.rcmap[i]);
        break;
    case MSP_SET_RCMAP:
        headSerialReply(0);
        for (i = 0; i < MAX_INPUTS; i++)
            mcfg.rcmap[i] = read8();
        break;

    case MSP_REBOOT:
        headSerialReply(0);
        pendReboot = true;
        break;
    case MSP_SILENCE_OSD:
        headSerialReply(0);
        osdSilence = true;
        break;
    default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
Example #12
0
/* Subroutine */ 
int qkfit(doublereal *umat, doublereal *rtsum, doublereal *r, integer *entry_) {

    /* Initialized data */

    static doublereal eps = 1e-5;
    static doublereal pi = 3.14159265358979;
    static doublereal one = 1.;
    static doublereal two = 2.;
    static doublereal three = 3.;
    static doublereal half = .5;
    static doublereal third = .333333333;
    static doublereal forthr = 1.333333333;
    static struct {
       doublereal fill_1[1];
       doublereal e_2[2];
       doublereal fill_3[2];
       doublereal e_4;
       doublereal fill_5[3];
	} equiv_6 = { {0}, {0.0}, {0.0}, 0, {0.0} };


    /* System generated locals */
    integer i_1;
    static doublereal equiv_7[9];

    /* Builtin functions */
    double sqrt(), d_sign(), atan(), cos();

    /* Local variables */
    static doublereal diff;
    static integer isig;
    static doublereal detu, root[3];
#define a ((doublereal *)&equiv_6)
#define b (equiv_7)
    static integer i, j, k;
    static doublereal s, t;
    extern /* Subroutine */ int eigen_();
    static doublereal digav, theta, argsq, b1, b2;
    extern /* Subroutine */ int esort_();
    static doublereal cos3th, cc, b13, dd, b23;
    static integer ia;
    static doublereal b33, qq, rt;
#define usqmat ((doublereal *)&equiv_6)
#define aam ((doublereal *)&equiv_6)
#define bam ((doublereal *)&equiv_6 + 4)
#define cam ((doublereal *)&equiv_6 + 8)
#define fam ((doublereal *)&equiv_6 + 7)
#define gam ((doublereal *)&equiv_6 + 6)
#define ham ((doublereal *)&equiv_6 + 3)
    static doublereal du11, du21, du31;
#define utr (equiv_7)





    /* Parameter adjustments */
    r -= 4;
    umat -= 4;

    /* Function Body */
    isig = 1;
    if (*entry_ == 1) {
	goto L200;
    }
/*     CALC DET OF UMAT */

    du11 = umat[8] * umat[12] - umat[11] * umat[9];
    du21 = umat[11] * umat[6] - umat[5] * umat[12];
    du31 = umat[5] * umat[9] - umat[8] * umat[6];
    detu = umat[4] * du11 + umat[7] * du21 + umat[10] * du31;
    if (detu < 0.) {
	isig = -1;
    }

/*     FORM USQMAT AS POSITIVE SEMI DEFINITE MATRIX */

    for (j = 1; j <= 3; ++j) {
	i_1 = j;
	for (i = 1; i <= i_1; ++i) {
	    usqmat[i + j * 3 - 4] = umat[i * 3 + 1] * umat[j * 3 + 1] + umat[
		    i * 3 + 2] * umat[j * 3 + 2] + umat[i * 3 + 3] * umat[j * 
		    3 + 3];
/* L105: */
	}
/* L110: */
    }
/* %    WRITE(6,999) USQMAT */

/*     REDUCE AVG OF DIAGONAL TERMS TO ZERO */

    digav = (*aam + *bam + *cam) * third;
/* %    WRITE(6,999) DIGAV */
    *aam -= digav;
    *bam -= digav;
    *cam -= digav;

/*     SETUP COEFFS OF SECULAR EQUATION OF MATRIX WITH TRACE ZERO */

    cc = *fam * *fam + *gam * *gam + *ham * *ham - *aam * *bam - *bam * *cam 
	    - *cam * *aam;
    dd = *aam * *bam * *cam + two * (*fam * *gam * *ham) - *aam * *fam * *fam 
	    - *bam * *gam * *gam - *cam * *ham * *ham;

/*     THE SECULAR EQN IS Y**3-CC*Y-DD=0  AND DD IS DET(USQMAT) */
/*     REDUCE THIS TO THE FORM COS**3-(3/4)COS- */
/*     (1/4)COS3THETA = 0 */
/*     WITH SOLUTIONS COSTHETA.  SO Y=QQ*COSTHETA */

    if (cc <= eps) {
	goto L115;
    }
    qq = sqrt(forthr * cc);
    cos3th = three * dd / (cc * qq);
    if (abs(cos3th) > one) {
/*	cos3th = d_sign(&one, &cos3th);  */
/*      Change suggested by Andrew Torda with many thanks, etc. eliminates the need for the FORTRAN libraries */
	cos3th = (cos3th > 0 ? one:-one); 

    }

/*     FUNCTION ARCOS */

    if (cos3th != 0.) {
	goto L1200;
    }
/* L1100: */
    theta = (float)1.570796327;
    goto L1400;
L1200:
    argsq = cos3th * cos3th;
    theta = atan(sqrt((float)1. - argsq) / cos3th);
    if (cos3th < 0.) {
	theta = pi - theta;
    }
L1400:

/*     ROOTS IN ORDER OF SIZE GO 1,2,3 1 LARGEST */

    theta *= third;
    root[0] = qq * cos(theta);
    diff = half * sqrt(three * (qq * qq - root[0] * root[0]));
    root[1] = -root[0] * half + diff;
    root[2] = -root[0] * half - diff;
    goto L120;
L115:

/*     SPECIAL FOR TRIPLY DEGENERATE */

    root[0] = (float)0.;
    root[1] = (float)0.;
    root[2] = (float)0.;
L120:
/*     ADD ON DIGAV AND TAKE SQRT */
    for (j = 1; j <= 3; ++j) {
	rt = root[j - 1] + digav;
	if (rt < eps) {
	    rt = (float)0.;
	}
	root[j - 1] = sqrt(rt);
/* L125: */
    }
/* %    WRITE(6,999) ROOT */
/*     IF DETU IS <0 CHANGE SIGN OF ROOT(3) */
    if (isig == -1) {
	root[2] = -root[2];
    }
    *rtsum = root[0] + root[1] + root[2];
/* %    WRITE(6,999) RTSUM */
    return 0;

/*     THIS IS THE FANCY PART */

L200:

/*     FORM USQ = (UT).U    (IN UPPER TRIANGULAR SYMMETRIC STORAGE MODE) 
*/

    for (i = 1; i <= 3; ++i) {
	for (j = i; j <= 3; ++j) {
/* SMJS Changed to be like Robs version */
	    t = (doublereal)0.;
	    for (k = 1; k <= 3; ++k) {
		t += umat[k + i * 3] * umat[k + j * 3];
/* L210: */
	    }
	    ia = i + (j * j - j) / 2;
	    utr[ia - 1] = t;
/* L220: */
	}
    }
/* %    WRITE(6,999) UTR */

/*     CALCULATE EIGENVALUES AND VECTORS */

    eigen_(utr, a, &c__3, &c__0);
    esort_(utr, a, &c__3, &c__0);
/* %    WRITE(6,999) UTR */

    root[0] = utr[0];
    root[1] = utr[2];
    root[2] = utr[5];
/* %    WRITE(6,999) ROOT */
/* %    WRITE(6,999) A */

/*     SET A3 = A1 CROSS A2 */
/*     ROOTS ARE IN ORDER R(1) >= R(2) >= R(3) >= 0 */

    a[6] = a[1] * a[5] - a[2] * a[4];
    a[7] = a[2] * a[3] - a[0] * a[5];
    a[8] = a[0] * a[4] - a[1] * a[3];
/* %    WRITE(6,999) A */

/*     VECTOR SET B=U.A */

    for (i = 1; i <= 3; ++i) {
	for (j = 1; j <= 3; ++j) {
/* SMJS Changed to be like Robs version */
	    t = (doublereal)0.;
	    for (k = 1; k <= 3; ++k) {
/* L230: */
		t += umat[j + k * 3] * a[k + i * 3 - 4];
	    }
	    b[j + i * 3 - 4] = t;
/* L240: */
	}
    }

/*      NORMALIZE B1 AND B2 AND CALCULATE B3 = B1 CROSS B2 */

    b1 = sqrt(b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
    b2 = sqrt(b[3] * b[3] + b[4] * b[4] + b[5] * b[5]);
    for (i = 1; i <= 3; ++i) {
	b[i - 1] /= b1;
/* L250: */
	b[i + 2] /= b2;
    }

/*      CHECK FOR LEFT HANDED ROTATION */

    b13 = b[1] * b[5] - b[2] * b[4];
    b23 = b[2] * b[3] - b[0] * b[5];
    b33 = b[0] * b[4] - b[1] * b[3];

    s = b13 * b[6] + b23 * b[7] + b33 * b[8];
    if (s < 0.) {
	isig = -1;
    }
    b[6] = b13;
    b[7] = b23;
    b[8] = b33;
/* %    WRITE(6,999) B */

/*     CALCULATE ROTATION MATRIX R */

    for (i = 1; i <= 3; ++i) {
	for (j = 1; j <= 3; ++j) {
/* SMJS Changed to be like Robs version */
	    t = (doublereal)0.;
	    for (k = 1; k <= 3; ++k) {
/* L260: */
		t += b[i + k * 3 - 4] * a[j + k * 3 - 4];
	    }
	    r[i + j * 3] = t;
/* L270: */
	}
    }

/*     RMS ERROR */

    for (i = 1; i <= 3; ++i) {
	if (root[i - 1] < 0.) {
	    root[i - 1] = (float)0.;
	}
	root[i - 1] = sqrt(root[i - 1]);
/* L280: */
    }

/*     CHANGE SIGN OF EVAL #3 IF LEFT HANDED */

    if (isig < 0) {
	root[2] = -root[2];
    }
    *rtsum = root[2] + root[1] + root[0];
    return 0;
} /* qkfit_ */
Example #13
0
void TaskDialog::setCurrentTaskInfo( TaskInfo* var )
{
	currentTaskInfo = var;
	if (!isShow())
	{
		toggle();
	}
	if (!var)
	{
		return;
	}
	//cocos2d::CCLabelTTF* taskTitle = dynamic_cast<cocos2d::CCLabelTTF*>(getChildByTag(10));
	//if (taskTitle)
	//{
	//	taskTitle->setString(var->m_taskDetail.m_titlett.c_str());
	//}
	//头像
	char str[50];
	if (currentTaskInfo->status==QUEST_STATUS_COMPLETE)
	{
		std::sprintf(str,"%d.png",currentTaskInfo->m_taskDetail.m_completeNPC);
	}
	else
	{
		std::sprintf(str,"%d.png",currentTaskInfo->m_taskDetail.m_anwserNPC);
	}
	touxiang->setSpriteFrame(SpriteFrameCache::getInstance()->getSpriteFrameByName(str));
	WXRichLabelEx* taskLabel = WXRichLabelEx::create();
	taskLabel->Initialize(FONT_ALIAS_DEFAULT_24,24, tableViewSize);
	WXRichLabelEx* oldTaskLabel = dynamic_cast<WXRichLabelEx*>(tableViewCellNode->getChildByTag(50));
	if (oldTaskLabel)
	{
		tableViewCellNode->removeChild(oldTaskLabel, true);
	}
	taskLabel->setTag(50);
	taskLabel->setVisible(true);
	tableViewCellNode->addChild(taskLabel);

	taskLabel->AddString(var->m_taskDetail.m_titlett.c_str(), CH_YELLOW, 0);
	taskLabel->EnterPos();
	if (var->status==QUEST_STATUS_AVAILABLE)
	{
		taskLabel->AddString(var->m_taskDetail.m_ot_q.c_str(), CH_WHITE, 0);
	}
	else if (var->status==QUEST_STATUS_COMPLETE)
	{
		taskLabel->AddString(var->m_taskDetail.m_et_q.c_str(), CH_WHITE, 0);
	}
	else if (var->status==QUEST_STATUS_INCOMPLETE)
	{
		taskLabel->AddString(var->m_taskDetail.m_it_q.c_str(),CH_WHITE,0);
	}
	//taskLabel->AddString(var->m_taskDetail.m_it_q, ccMAGENTA, 0);
	//tableViewCellNode->setContentSize(CCSizeMake(tableViewSize.width, abs(taskLabel->GetCurrentHeight())));
	//awardLabel->ResetContent();
	taskLabel->EnterPos();
	taskLabel->EnterPos();
	int middleHeight=abs(taskLabel->GetCurrentHeight());
	if(var->status!=QUEST_STATUS_INCOMPLETE)//如果任务不是未完成,再提示奖励信息
	{
		taskLabel->EnterPos();
		taskLabel->AddString(taskAwardStr.c_str(), CH_YELLOW, 0);
		taskLabel->EnterPos();

		char str[_MAX_PATH_];
		if (var->m_taskDetail.m_money_num>0)
		{
			switch (var->m_taskDetail.m_money_type)
			{
			case MONEY_BIND_TYPE:
				taskLabel->AddString((GETSTR("general_label_0036")+maohaoStr).c_str(), CH_YELLOW, 0);
				break;
			case MONEY_NO_BIND_TYPE:
				taskLabel->AddString((GETSTR("general_label_0037")+maohaoStr).c_str(), CH_YELLOW, 0);
				break;
			case MONEY_GUILD_DEVOTE:
				taskLabel->AddString((GETSTR("general_label_0039")+maohaoStr).c_str(), CH_YELLOW, 0);
				break;
			}
			memset(str, 0, sizeof(str));
			sprintf(str, "%d", var->m_taskDetail.m_money_num);
			taskLabel->AddString(str, CH_WHITE, 0);
			taskLabel->EnterPos();
		}
		if (var->m_taskDetail.m_exp_num>0)
		{
			taskLabel->AddString(expStr+maohaoStr, CH_YELLOW, 0);
			memset(str, 0, sizeof(str));
			sprintf(str, "%d", var->m_taskDetail.m_exp_num);
			taskLabel->AddString(str, CH_WHITE, 0);
			taskLabel->EnterPos();
			taskLabel->EnterPos();
		}

		std::vector<WXGrid*>::iterator gridItr = bounsGridPanel->getAllGrid()->begin();
		for (;gridItr!=bounsGridPanel->getAllGrid()->end();gridItr++)
		{
			(*gridItr)->setVisible(false);
		}
		gridItr = bounsGridPanel->getAllGrid()->begin();
		/*if (var->m_taskDetail.m_goods_id>0)
		{
		tGridData tgd;
		tgd.isActive = true;
		tgd.num = var->m_taskDetail.m_goods_num;
		tgd.pos = 0;
		tgd.id=var->m_taskDetail.m_goods_id;
		ItemConfigData* cid = DataManager::getInstance().getItemConfigManager()->getOneData(var->m_taskDetail.m_goods_id);
		if (!cid)
		{
		sprintf(str, "%s", GRID_BLANK_FILENAME);
		}
		else
		{
		sprintf(str, "ui/itemimage/%d.pvr.ccz", cid->displayid);
		}
		tgd.url = str;
		(*gridItr)->updateGrid(tgd);
		(*gridItr)->setVisible(true);
		}*/
		int y = abs(taskLabel->GetCurrentHeight());
		fengexian->setPosition(ccp(tableViewSize.width/2,-middleHeight));
		y = y+(*gridItr)->getContentSize().height;
		bounsGridPanel->setPosition(ccp(0,-y+(*gridItr)->getContentSize().height/2));
		tableViewCellNode->setContentSize(CCSizeMake(tableViewSize.width, y));
	}
	else//如果任务是未完成的,不显示
	{
		std::vector<WXGrid*>::iterator gridItr = bounsGridPanel->getAllGrid()->begin();
		for (;gridItr!=bounsGridPanel->getAllGrid()->end();gridItr++)
		{
			(*gridItr)->setVisible(false);
		}
	}
	WXMenu* menu = dynamic_cast<WXMenu*>(getChildByTag(1000));
	if (menu)
	{
		//WXSimpleButton* leftBtn = dynamic_cast<WXSimpleButton*>(menu->getChildByTag(1));
		//if (leftBtn)
		//{
		//	leftBtn->setVisible(false);
		//}
		WXSimpleButton* rightBtn = dynamic_cast<WXSimpleButton*>(menu->getChildByTag(2));
		if (rightBtn)
		{
			//任务状态
			if( var->status == QUEST_STATUS_COMPLETE )
			{
				rightBtn->setString(finishStr.c_str());
			}
			else if( var->status == QUEST_STATUS_INCOMPLETE )
			{
				rightBtn->setString(leaveStr.c_str());
			}
			else if( var->status == QUEST_STATUS_AVAILABLE )
			{
				rightBtn->setString(acceptStr.c_str());
			}
			else
			{
				rightBtn->setString(leaveStr.c_str());
			}
		}
	}
	if (tableView)
	{
		tableView->reloadData();
	}
}
Example #14
0
void P_BobWeapon (player_t *player, pspdef_t *psp, fixed_t *x, fixed_t *y)
{
	static fixed_t curbob;

	AWeapon *weapon;
	fixed_t bobtarget;

	weapon = player->ReadyWeapon;

	if (weapon == NULL || weapon->WeaponFlags & WIF_DONTBOB)
	{
		*x = *y = 0;
		return;
	}

	// [XA] Get the current weapon's bob properties.
	int bobstyle = weapon->BobStyle;
	int bobspeed = (weapon->BobSpeed * 128) >> 16;
	fixed_t rangex = weapon->BobRangeX;
	fixed_t rangey = weapon->BobRangeY;

	// Bob the weapon based on movement speed.
	int angle = (bobspeed*35/TICRATE*level.time)&FINEMASK;

	// [RH] Smooth transitions between bobbing and not-bobbing frames.
	// This also fixes the bug where you can "stick" a weapon off-center by
	// shooting it when it's at the peak of its swing.
	bobtarget = (player->WeaponState & WF_WEAPONBOBBING) ? player->bob : 0;
	if (curbob != bobtarget)
	{
		if (abs (bobtarget - curbob) <= 1*FRACUNIT)
		{
			curbob = bobtarget;
		}
		else
		{
			fixed_t zoom = MAX<fixed_t> (1*FRACUNIT, abs (curbob - bobtarget) / 40);
			if (curbob > bobtarget)
			{
				curbob -= zoom;
			}
			else
			{
				curbob += zoom;
			}
		}
	}

	if (curbob != 0)
	{
		fixed_t bobx = FixedMul(player->bob, rangex);
		fixed_t boby = FixedMul(player->bob, rangey);
		switch (bobstyle)
		{
		case AWeapon::BobNormal:
			*x = FixedMul(bobx, finecosine[angle]);
			*y = FixedMul(boby, finesine[angle & (FINEANGLES/2-1)]);
			break;
			
		case AWeapon::BobInverse:
			*x = FixedMul(bobx, finecosine[angle]);
			*y = boby - FixedMul(boby, finesine[angle & (FINEANGLES/2-1)]);
			break;
			
		case AWeapon::BobAlpha:
			*x = FixedMul(bobx, finesine[angle]);
			*y = FixedMul(boby, finesine[angle & (FINEANGLES/2-1)]);
			break;
			
		case AWeapon::BobInverseAlpha:
			*x = FixedMul(bobx, finesine[angle]);
			*y = boby - FixedMul(boby, finesine[angle & (FINEANGLES/2-1)]);
			break;
			
		case AWeapon::BobSmooth:
			*x = FixedMul(bobx, finecosine[angle]);
			*y = (boby - FixedMul(boby, finecosine[angle*2 & (FINEANGLES-1)])) / 2;
			break;

		case AWeapon::BobInverseSmooth:
			*x = FixedMul(bobx, finecosine[angle]);
			*y = (FixedMul(boby, finecosine[angle*2 & (FINEANGLES-1)]) + boby) / 2;
		}
	}
	else
	{
		*x = 0;
		*y = 0;
	}
}
Example #15
0
File: Audio.cpp Project: tschw/hifi
inline void Audio::analyzePing() {

    // Determine extrema
    int botAt = findExtremum(_echoSamplesLeft, PING_SAMPLES_TO_ANALYZE, -1);
    if (botAt == -1) {
        printLog("Audio Ping: Minimum not found.\n");
        return;
    }
    int topAt = findExtremum(_echoSamplesLeft, PING_SAMPLES_TO_ANALYZE, 1);
    if (topAt == -1) {
        printLog("Audio Ping: Maximum not found.\n");
        return;
    }

    // Determine peak amplitude - warn if low
    int ampli = (_echoSamplesLeft[topAt] - _echoSamplesLeft[botAt]) / 2;
    if (ampli < PING_MIN_AMPLI) {
        printLog("Audio Ping unreliable - low amplitude %d.\n", ampli);
    }

    // Determine period - warn if doesn't look like our signal
    int halfPeriod = abs(topAt - botAt);
    if (abs(halfPeriod-PING_HALF_PERIOD) > PING_MAX_PERIOD_DIFFERENCE) {
        printLog("Audio Ping unreliable - peak distance %d vs. %d\n", halfPeriod, PING_HALF_PERIOD);
    }

    // Ping is sent:
    //
    // ---[ record ]--[  play  ]--- audio in space/time --->
    //    :        :           :
    //    :        : ping: ->X<-
    //    :        :         :
    //    :        :         |+| (buffer end - signal center = t1-t0)
    //    :      |<----------+
    //    :      : :         :
    //    :    ->X<- (corresponding input buffer position t0)
    //    :      : :         :
    //    :      : :         :
    //    :      : :         :
    // Next frame (we're recording from now on):
    //    :      : :
    //    : -  - --[ record ]--[  play  ]------------------>
    //    :      : :         :
    //    :      : |<-- (start of recording t1)
    //    :      : :
    //    :      : :
    // At some frame, the signal is picked up:
    //    :      : :         :
    //    :      : :         : 
    //    :      : :         V
    //    :      : : -  - --[ record ]--[  play  ]---------->
    //    :      V :         :
    //    :      |<--------->|
    //           |+|<------->|  period + measured samples
    //
    // If we could pick up the signal at t0 we'd have zero round trip
    // time - in this case we had recorded the output buffer instantly
    // in its entirety (we can't - but there's the proper reference
    // point). We know the number of samples from t1 and, knowing that
    // data is streaming continuously, we know that t1-t0 is the distance
    // of the characterisic point from the end of the buffer.

    int delay = (botAt + topAt) / 2 + PING_PERIOD;

    printLog("\n| Audio Ping results:\n+----- ---- --- - -  -   -   -\n\n"
             "Delay = %d samples (%d ms)\nPeak amplitude = %d\n\n",
             delay, (delay * 1000) / int(SAMPLE_RATE), ampli);
}
Example #16
0
/********************************************************************************
				初始化查找表
********************************************************************************/
void  CDescriptor::InitializeLUT()
{
	//先计算0度角的点
	int nC = nMaxRegionNum*nH/2;
	for(int i = 0;i<nMaxRegionNum; i++)
		for(int j = 0; j < nH; j++)
			for(int k=0; k < nW; k++)
			{
				int temp = nW*nH*i + j*nW +k;
				LUTSubRegion[0][temp].nNo1 = k-(nW-1)/2;
				LUTSubRegion[0][temp].nNo2 = i*nH + j - nC;
			}
	
	//计算各旋转位置
	for(int i= 1;i < 360; i++)
	{	
		double dArc = -(double)(i*PI/180);
		for(int j= 0; j < nMaxRegionNum*nEachRPixes; j++)
			{
				int xx = LUTSubRegion[0][j].nNo1;
				int yy = LUTSubRegion[0][j].nNo2;
				LUTSubRegion[i][j].nNo1 = wzhRound(xx*cos(dArc) - yy*sin(dArc));
				LUTSubRegion[i][j].nNo2 = wzhRound(xx*sin(dArc) + yy*cos(dArc));
			}
	}

	//自相关
	int nDim = 0;
	for(int i = 0; i < nMaxRegionNum; i++)
	{
		LUTDes2Region[nDim].nNo1 = i;
		LUTDes2Region[nDim].nNo2 = i;
		nDim ++;
	}

	//互相关
	for(int i = 0; i < nMaxRegionNum-1; i++)
	{
		LUTDes2Region[nDim].nNo1 = i;
		LUTDes2Region[nDim].nNo2 = i+1;
		nDim ++;
		LUTDes2Region[nDim].nNo1 = i+1;
		LUTDes2Region[nDim].nNo2 = i;
		nDim ++;
	}

	//
	double dSigma = 22.0;
	int nR = (nH-1)/2;
	for(int i=0; i<nMaxRegionNum; i++)
		for(int j=0; j<nH; j++)
			for(int k=0; k<nW;k++)
			{
				int P = i*nEachRPixes + j*nW + k;
				int nNo1 = 0;
				int nNo2 = 0;
				double dCoe1 = 0;
				if(j < nR)
				{
					nNo1 = i-1;
					nNo2 = nNo1 + 1;
					dCoe1 = double(nR-j)/nH;
				}
				else if(j == nR)
				{
					nNo1 = i;
					nNo2 = i;
					dCoe1 = 1;
				}
				else
				{
					nNo1 = i;
					nNo2 = nNo1 + 1;
					dCoe1 = 1 - double(j-nR)/nH;
				}
				
				//特殊
				if(nNo1 == -1)
				{
					nNo1 = 0;
					dCoe1 = 1;
				}
				if(nNo2 == nMaxRegionNum)
				{
					nNo2	= nMaxRegionNum-1;
					dCoe1	= 0;
				}

				LUTBiPos[P].nNo1	= nNo1;
				LUTBiPos[P].nNo2	= nNo2;
				LUTBiPos[P].dCoe1	= dCoe1;
				LUTBiPos[P].dCoe2	= 1-dCoe1;

				int nC		 = (nH*nMaxRegionNum-1)/2;
				double	d	 = (double)abs(i*nH+j-nC);
				LUTWeight[P] = exp(-d*d/(2*dSigma*dSigma));
			}
}
Example #17
0
File: Audio.cpp Project: tschw/hifi
inline void Audio::performIO(int16_t* inputLeft, int16_t* outputLeft, int16_t* outputRight) {

    NodeList* nodeList = NodeList::getInstance();
    Application* interface = Application::getInstance();
    Avatar* interfaceAvatar = interface->getAvatar();
 
    // Add Procedural effects to input samples
    addProceduralSounds(inputLeft, BUFFER_LENGTH_SAMPLES_PER_CHANNEL);
    
    if (nodeList && inputLeft) {
        
        //  Measure the loudness of the signal from the microphone and store in audio object
        float loudness = 0;
        for (int i = 0; i < BUFFER_LENGTH_SAMPLES_PER_CHANNEL; i++) {
            loudness += abs(inputLeft[i]);
        }
        
        loudness /= BUFFER_LENGTH_SAMPLES_PER_CHANNEL;
        _lastInputLoudness = loudness;
        
        // add input (@microphone) data to the scope
        _scope->addSamples(0, inputLeft, BUFFER_LENGTH_SAMPLES_PER_CHANNEL);

        Node* audioMixer = nodeList->soloNodeOfType(NODE_TYPE_AUDIO_MIXER);
        
        if (audioMixer) {
            glm::vec3 headPosition = interfaceAvatar->getHeadJointPosition();
            glm::quat headOrientation = interfaceAvatar->getHead().getOrientation();
            
            int numBytesPacketHeader = numBytesForPacketHeader((unsigned char*) &PACKET_TYPE_MICROPHONE_AUDIO_NO_ECHO);
            int leadingBytes = numBytesPacketHeader + sizeof(headPosition) + sizeof(headOrientation);
            
            // we need the amount of bytes in the buffer + 1 for type
            // + 12 for 3 floats for position + float for bearing + 1 attenuation byte
            unsigned char dataPacket[BUFFER_LENGTH_BYTES_PER_CHANNEL + leadingBytes];
            
            PACKET_TYPE packetType = (Application::getInstance()->shouldEchoAudio())
                ? PACKET_TYPE_MICROPHONE_AUDIO_WITH_ECHO
                : PACKET_TYPE_MICROPHONE_AUDIO_NO_ECHO;
            
            unsigned char* currentPacketPtr = dataPacket + populateTypeAndVersion(dataPacket, packetType);
            
            // memcpy the three float positions
            memcpy(currentPacketPtr, &headPosition, sizeof(headPosition));
            currentPacketPtr += (sizeof(headPosition));
            
            // memcpy our orientation
            memcpy(currentPacketPtr, &headOrientation, sizeof(headOrientation));
            currentPacketPtr += sizeof(headOrientation);
            
            // copy the audio data to the last BUFFER_LENGTH_BYTES bytes of the data packet
            memcpy(currentPacketPtr, inputLeft, BUFFER_LENGTH_BYTES_PER_CHANNEL);
            nodeList->getNodeSocket()->send(audioMixer->getActiveSocket(),
                                              dataPacket,
                                              BUFFER_LENGTH_BYTES_PER_CHANNEL + leadingBytes);

            interface->getBandwidthMeter()->outputStream(BandwidthMeter::AUDIO)
                    .updateValue(BUFFER_LENGTH_BYTES_PER_CHANNEL + leadingBytes);
        }
        
    }
    
    memset(outputLeft, 0, PACKET_LENGTH_BYTES_PER_CHANNEL);
    memset(outputRight, 0, PACKET_LENGTH_BYTES_PER_CHANNEL);
    
    AudioRingBuffer* ringBuffer = &_ringBuffer;
    
    // if there is anything in the ring buffer, decide what to do:
    
    if (ringBuffer->getEndOfLastWrite()) {
        if (!ringBuffer->isStarted() && ringBuffer->diffLastWriteNextOutput() <
            (PACKET_LENGTH_SAMPLES + _jitterBufferSamples * (ringBuffer->isStereo() ? 2 : 1))) {
            //
            //  If not enough audio has arrived to start playback, keep waiting
            //
#ifdef SHOW_AUDIO_DEBUG
            printLog("%i,%i,%i,%i\n",
                     _packetsReceivedThisPlayback,
                     ringBuffer->diffLastWriteNextOutput(),
                     PACKET_LENGTH_SAMPLES,
                     _jitterBufferSamples);
#endif
        } else if (ringBuffer->isStarted() && ringBuffer->diffLastWriteNextOutput() == 0) {
            //
            //  If we have started and now have run out of audio to send to the audio device, 
            //  this means we've starved and should restart.  
            //  
            ringBuffer->setStarted(false);
            
            _numStarves++;
            _packetsReceivedThisPlayback = 0;
            _wasStarved = 10;          //   Frames for which to render the indication that the system was starved.
#ifdef SHOW_AUDIO_DEBUG
            printLog("Starved, remaining samples = %d\n",
                     ringBuffer->diffLastWriteNextOutput());
#endif

        } else {
            //
            //  We are either already playing back, or we have enough audio to start playing back.
            // 
            if (!ringBuffer->isStarted()) {
                ringBuffer->setStarted(true);
#ifdef SHOW_AUDIO_DEBUG
                printLog("starting playback %0.1f msecs delayed, jitter = %d, pkts recvd: %d \n",
                         (usecTimestampNow() - usecTimestamp(&_firstPacketReceivedTime))/1000.0,
                         _jitterBufferSamples,
                         _packetsReceivedThisPlayback);
#endif
            }

            //
            // play whatever we have in the audio buffer
            //
            // if we haven't fired off the flange effect, check if we should
            // TODO: lastMeasuredHeadYaw is now relative to body - check if this still works.
            
            int lastYawMeasured = fabsf(interfaceAvatar->getHeadYawRate());
            
            if (!_samplesLeftForFlange && lastYawMeasured > MIN_FLANGE_EFFECT_THRESHOLD) {
                // we should flange for one second
                if ((_lastYawMeasuredMaximum = std::max(_lastYawMeasuredMaximum, lastYawMeasured)) != lastYawMeasured) {
                    _lastYawMeasuredMaximum = std::min(_lastYawMeasuredMaximum, MIN_FLANGE_EFFECT_THRESHOLD);
                    
                    _samplesLeftForFlange = SAMPLE_RATE;
                    
                    _flangeIntensity = MIN_FLANGE_INTENSITY +
                        ((_lastYawMeasuredMaximum - MIN_FLANGE_EFFECT_THRESHOLD) /
                         (float)(MAX_FLANGE_EFFECT_THRESHOLD - MIN_FLANGE_EFFECT_THRESHOLD)) *
                        (1 - MIN_FLANGE_INTENSITY);
                    
                    _flangeRate = FLANGE_BASE_RATE * _flangeIntensity;
                    _flangeWeight = MAX_FLANGE_SAMPLE_WEIGHT * _flangeIntensity;
                }
            }
            
            for (int s = 0; s < PACKET_LENGTH_SAMPLES_PER_CHANNEL; s++) {
                
                int leftSample = ringBuffer->getNextOutput()[s];
                int rightSample = ringBuffer->getNextOutput()[s + PACKET_LENGTH_SAMPLES_PER_CHANNEL];
                
                if (_samplesLeftForFlange > 0) {
                    float exponent = (SAMPLE_RATE - _samplesLeftForFlange - (SAMPLE_RATE / _flangeRate)) /
                        (SAMPLE_RATE / _flangeRate);
                    int sampleFlangeDelay = (SAMPLE_RATE / (1000 * _flangeIntensity)) * powf(2, exponent);
                    
                    if (_samplesLeftForFlange != SAMPLE_RATE || s >= (SAMPLE_RATE / 2000)) {
                        // we have a delayed sample to add to this sample
                        
                        int16_t *flangeFrame = ringBuffer->getNextOutput();
                        int flangeIndex = s - sampleFlangeDelay;
                        
                        if (flangeIndex < 0) {
                            // we need to grab the flange sample from earlier in the buffer
                            flangeFrame = ringBuffer->getNextOutput() != ringBuffer->getBuffer()
                            ? ringBuffer->getNextOutput() - PACKET_LENGTH_SAMPLES
                            : ringBuffer->getNextOutput() + RING_BUFFER_LENGTH_SAMPLES - PACKET_LENGTH_SAMPLES;
                            
                            flangeIndex = PACKET_LENGTH_SAMPLES_PER_CHANNEL + (s - sampleFlangeDelay);
                        }
                        
                        int16_t leftFlangeSample = flangeFrame[flangeIndex];
                        int16_t rightFlangeSample = flangeFrame[flangeIndex + PACKET_LENGTH_SAMPLES_PER_CHANNEL];
                        
                        leftSample = (1 - _flangeWeight) * leftSample + (_flangeWeight * leftFlangeSample);
                        rightSample = (1 - _flangeWeight) * rightSample + (_flangeWeight * rightFlangeSample);
                        
                        _samplesLeftForFlange--;
                        
                        if (_samplesLeftForFlange == 0) {
                            _lastYawMeasuredMaximum = 0;
                        }
                    }
                }
#ifndef TEST_AUDIO_LOOPBACK
                outputLeft[s] = leftSample;
                outputRight[s] = rightSample;
#else 
                outputLeft[s] = inputLeft[s];
                outputRight[s] = inputLeft[s];                    
#endif
            }
            ringBuffer->setNextOutput(ringBuffer->getNextOutput() + PACKET_LENGTH_SAMPLES);
            
            if (ringBuffer->getNextOutput() == ringBuffer->getBuffer() + RING_BUFFER_LENGTH_SAMPLES) {
                ringBuffer->setNextOutput(ringBuffer->getBuffer());
            }
        }
    }

    eventuallySendRecvPing(inputLeft, outputLeft, outputRight);


    // add output (@speakers) data just written to the scope
    _scope->addSamples(1, outputLeft, BUFFER_LENGTH_SAMPLES_PER_CHANNEL);
    _scope->addSamples(2, outputRight, BUFFER_LENGTH_SAMPLES_PER_CHANNEL);

    gettimeofday(&_lastCallbackTime, NULL);
}
Example #18
0
/* Subroutine */ int dlasd6_(integer *icompq, integer *nl, integer *nr,
                             integer *sqre, doublereal *d__, doublereal *vf, doublereal *vl,
                             doublereal *alpha, doublereal *beta, integer *idxq, integer *perm,
                             integer *givptr, integer *givcol, integer *ldgcol, doublereal *givnum,
                             integer *ldgnum, doublereal *poles, doublereal *difl, doublereal *
                             difr, doublereal *z__, integer *k, doublereal *c__, doublereal *s,
                             doublereal *work, integer *iwork, integer *info)
{
    /* System generated locals */
    integer givcol_dim1, givcol_offset, givnum_dim1, givnum_offset,
            poles_dim1, poles_offset, i__1;
    doublereal d__1, d__2;

    /* Local variables */
    integer i__, m, n, n1, n2, iw, idx, idxc, idxp, ivfw, ivlw;
    integer isigma;
    doublereal orgnrm;

    /*  -- LAPACK auxiliary routine (version 3.2) -- */
    /*     November 2006 */

    /*  Purpose */
    /*  ======= */

    /*  DLASD6 computes the SVD of an updated upper bidiagonal matrix B */
    /*  obtained by merging two smaller ones by appending a row. This */
    /*  routine is used only for the problem which requires all singular */
    /*  values and optionally singular vector matrices in factored form. */
    /*  B is an N-by-M matrix with N = NL + NR + 1 and M = N + SQRE. */
    /*  A related subroutine, DLASD1, handles the case in which all singular */
    /*  values and singular vectors of the bidiagonal matrix are desired. */

    /*  DLASD6 computes the SVD as follows: */

    /*                ( D1(in)  0    0     0 ) */
    /*    B = U(in) * (   Z1'   a   Z2'    b ) * VT(in) */
    /*                (   0     0   D2(in) 0 ) */

    /*      = U(out) * ( D(out) 0) * VT(out) */

    /*  where Z' = (Z1' a Z2' b) = u' VT', and u is a vector of dimension M */
    /*  with ALPHA and BETA in the NL+1 and NL+2 th entries and zeros */
    /*  elsewhere; and the entry b is empty if SQRE = 0. */

    /*  The singular values of B can be computed using D1, D2, the first */
    /*  components of all the right singular vectors of the lower block, and */
    /*  the last components of all the right singular vectors of the upper */
    /*  block. These components are stored and updated in VF and VL, */
    /*  respectively, in DLASD6. Hence U and VT are not explicitly */
    /*  referenced. */

    /*  The singular values are stored in D. The algorithm consists of two */
    /*  stages: */

    /*        The first stage consists of deflating the size of the problem */
    /*        when there are multiple singular values or if there is a zero */
    /*        in the Z vector. For each such occurence the dimension of the */
    /*        secular equation problem is reduced by one. This stage is */
    /*        performed by the routine DLASD7. */

    /*        The second stage consists of calculating the updated */
    /*        singular values. This is done by finding the roots of the */
    /*        secular equation via the routine DLASD4 (as called by DLASD8). */
    /*        This routine also updates VF and VL and computes the distances */
    /*        between the updated singular values and the old singular */
    /*        values. */

    /*  DLASD6 is called from DLASDA. */

    /*  Arguments */
    /*  ========= */

    /*  ICOMPQ (input) INTEGER */
    /*         Specifies whether singular vectors are to be computed in */
    /*         factored form: */
    /*         = 0: Compute singular values only. */
    /*         = 1: Compute singular vectors in factored form as well. */

    /*  NL     (input) INTEGER */
    /*         The row dimension of the upper block.  NL >= 1. */

    /*  NR     (input) INTEGER */
    /*         The row dimension of the lower block.  NR >= 1. */

    /*  SQRE   (input) INTEGER */
    /*         = 0: the lower block is an NR-by-NR square matrix. */
    /*         = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */

    /*         The bidiagonal matrix has row dimension N = NL + NR + 1, */
    /*         and column dimension M = N + SQRE. */

    /*  D      (input/output) DOUBLE PRECISION array, dimension ( NL+NR+1 ). */
    /*         On entry D(1:NL,1:NL) contains the singular values of the */
    /*         upper block, and D(NL+2:N) contains the singular values */
    /*         of the lower block. On exit D(1:N) contains the singular */
    /*         values of the modified matrix. */

    /*  VF     (input/output) DOUBLE PRECISION array, dimension ( M ) */
    /*         On entry, VF(1:NL+1) contains the first components of all */
    /*         right singular vectors of the upper block; and VF(NL+2:M) */
    /*         contains the first components of all right singular vectors */
    /*         of the lower block. On exit, VF contains the first components */
    /*         of all right singular vectors of the bidiagonal matrix. */

    /*  VL     (input/output) DOUBLE PRECISION array, dimension ( M ) */
    /*         On entry, VL(1:NL+1) contains the  last components of all */
    /*         right singular vectors of the upper block; and VL(NL+2:M) */
    /*         contains the last components of all right singular vectors of */
    /*         the lower block. On exit, VL contains the last components of */
    /*         all right singular vectors of the bidiagonal matrix. */

    /*  ALPHA  (input/output) DOUBLE PRECISION */
    /*         Contains the diagonal element associated with the added row. */

    /*  BETA   (input/output) DOUBLE PRECISION */
    /*         Contains the off-diagonal element associated with the added */
    /*         row. */

    /*  IDXQ   (output) INTEGER array, dimension ( N ) */
    /*         This contains the permutation which will reintegrate the */
    /*         subproblem just solved back into sorted order, i.e. */
    /*         D( IDXQ( I = 1, N ) ) will be in ascending order. */

    /*  PERM   (output) INTEGER array, dimension ( N ) */
    /*         The permutations (from deflation and sorting) to be applied */
    /*         to each block. Not referenced if ICOMPQ = 0. */

    /*  GIVPTR (output) INTEGER */
    /*         The number of Givens rotations which took place in this */
    /*         subproblem. Not referenced if ICOMPQ = 0. */

    /*  GIVCOL (output) INTEGER array, dimension ( LDGCOL, 2 ) */
    /*         Each pair of numbers indicates a pair of columns to take place */
    /*         in a Givens rotation. Not referenced if ICOMPQ = 0. */

    /*  LDGCOL (input) INTEGER */
    /*         leading dimension of GIVCOL, must be at least N. */

    /*  GIVNUM (output) DOUBLE PRECISION array, dimension ( LDGNUM, 2 ) */
    /*         Each number indicates the C or S value to be used in the */
    /*         corresponding Givens rotation. Not referenced if ICOMPQ = 0. */

    /*  LDGNUM (input) INTEGER */
    /*         The leading dimension of GIVNUM and POLES, must be at least N. */

    /*  POLES  (output) DOUBLE PRECISION array, dimension ( LDGNUM, 2 ) */
    /*         On exit, POLES(1,*) is an array containing the new singular */
    /*         values obtained from solving the secular equation, and */
    /*         POLES(2,*) is an array containing the poles in the secular */
    /*         equation. Not referenced if ICOMPQ = 0. */

    /*  DIFL   (output) DOUBLE PRECISION array, dimension ( N ) */
    /*         On exit, DIFL(I) is the distance between I-th updated */
    /*         (undeflated) singular value and the I-th (undeflated) old */
    /*         singular value. */

    /*  DIFR   (output) DOUBLE PRECISION array, */
    /*                  dimension ( LDGNUM, 2 ) if ICOMPQ = 1 and */
    /*                  dimension ( N ) if ICOMPQ = 0. */
    /*         On exit, DIFR(I, 1) is the distance between I-th updated */
    /*         (undeflated) singular value and the I+1-th (undeflated) old */
    /*         singular value. */

    /*         If ICOMPQ = 1, DIFR(1:K,2) is an array containing the */
    /*         normalizing factors for the right singular vector matrix. */

    /*         See DLASD8 for details on DIFL and DIFR. */

    /*  Z      (output) DOUBLE PRECISION array, dimension ( M ) */
    /*         The first elements of this array contain the components */
    /*         of the deflation-adjusted updating row vector. */

    /*  K      (output) INTEGER */
    /*         Contains the dimension of the non-deflated matrix, */
    /*         This is the order of the related secular equation. 1 <= K <=N. */

    /*  C      (output) DOUBLE PRECISION */
    /*         C contains garbage if SQRE =0 and the C-value of a Givens */
    /*         rotation related to the right null space if SQRE = 1. */

    /*  S      (output) DOUBLE PRECISION */
    /*         S contains garbage if SQRE =0 and the S-value of a Givens */
    /*         rotation related to the right null space if SQRE = 1. */

    /*  WORK   (workspace) DOUBLE PRECISION array, dimension ( 4 * M ) */

    /*  IWORK  (workspace) INTEGER array, dimension ( 3 * N ) */

    /*  INFO   (output) INTEGER */
    /*          = 0:  successful exit. */
    /*          < 0:  if INFO = -i, the i-th argument had an illegal value. */
    /*          > 0:  if INFO = 1, an singular value did not converge */

    /*  Further Details */
    /*  =============== */

    /*  Based on contributions by */
    /*     Ming Gu and Huan Ren, Computer Science Division, University of */
    /*     California at Berkeley, USA */

    /*  ===================================================================== */

    /*     Test the input parameters. */

    /* Parameter adjustments */
    --d__;
    --vf;
    --vl;
    --idxq;
    --perm;
    givcol_dim1 = *ldgcol;
    givcol_offset = 1 + givcol_dim1;
    givcol -= givcol_offset;
    poles_dim1 = *ldgnum;
    poles_offset = 1 + poles_dim1;
    poles -= poles_offset;
    givnum_dim1 = *ldgnum;
    givnum_offset = 1 + givnum_dim1;
    givnum -= givnum_offset;
    --difl;
    --difr;
    --z__;
    --work;
    --iwork;

    /* Function Body */
    *info = 0;
    n = *nl + *nr + 1;
    m = n + *sqre;

    if (*icompq < 0 || *icompq > 1) {
        *info = -1;
    } else if (*nl < 1) {
        *info = -2;
    } else if (*nr < 1) {
        *info = -3;
    } else if (*sqre < 0 || *sqre > 1) {
        *info = -4;
    } else if (*ldgcol < n) {
        *info = -14;
    } else if (*ldgnum < n) {
        *info = -16;
    }
    if (*info != 0) {
        i__1 = -(*info);
        xerbla_("DLASD6", &i__1);
        return 0;
    }

    /*     The following values are for bookkeeping purposes only.  They are */
    /*     integer pointers which indicate the portion of the workspace */
    /*     used by a particular array in DLASD7 and DLASD8. */

    isigma = 1;
    iw = isigma + n;
    ivfw = iw + m;
    ivlw = ivfw + m;

    idx = 1;
    idxc = idx + n;
    idxp = idxc + n;

    /*     Scale. */

    /* Computing MAX */
    d__1 = abs(*alpha), d__2 = abs(*beta);
    orgnrm = max(d__1,d__2);
    d__[*nl + 1] = 0.;
    i__1 = n;
    for (i__ = 1; i__ <= i__1; ++i__) {
        if ((d__1 = d__[i__], abs(d__1)) > orgnrm) {
            orgnrm = (d__1 = d__[i__], abs(d__1));
        }
    }
    dlascl_("G", &c__0, &c__0, &orgnrm, &c_b7, &n, &c__1, &d__[1], &n, info);
    *alpha /= orgnrm;
    *beta /= orgnrm;

    /*     Sort and Deflate singular values. */

    dlasd7_(icompq, nl, nr, sqre, k, &d__[1], &z__[1], &work[iw], &vf[1], &
            work[ivfw], &vl[1], &work[ivlw], alpha, beta, &work[isigma], &
            iwork[idx], &iwork[idxp], &idxq[1], &perm[1], givptr, &givcol[
                givcol_offset], ldgcol, &givnum[givnum_offset], ldgnum, c__, s,
            info);

    /*     Solve Secular Equation, compute DIFL, DIFR, and update VF, VL. */

    dlasd8_(icompq, k, &d__[1], &z__[1], &vf[1], &vl[1], &difl[1], &difr[1],
            ldgnum, &work[isigma], &work[iw], info);

    /*     Save the poles if ICOMPQ = 1. */

    if (*icompq == 1) {
        dcopy_(k, &d__[1], &c__1, &poles[poles_dim1 + 1], &c__1);
        dcopy_(k, &work[isigma], &c__1, &poles[(poles_dim1 << 1) + 1], &c__1);
    }

    /*     Unscale. */

    dlascl_("G", &c__0, &c__0, &c_b7, &orgnrm, &n, &c__1, &d__[1], &n, info);

    /*     Prepare the IDXQ sorting permutation. */

    n1 = *k;
    n2 = n - *k;
    dlamrg_(&n1, &n2, &d__[1], &c__1, &c_n1, &idxq[1]);

    return 0;

    /*     End of DLASD6 */

} /* dlasd6_ */
Example #19
0
int LuRatio(SizeVector &ip, SizeVector &jp, ADvector &LU, AD<Base> &ratio) //
{
	typedef ADvector FloatVector;                                       //
	typedef AD<Base>       Float;                                       //

	// check numeric type specifications
	CheckNumericType<Float>();

	// check simple vector class specifications
	CheckSimpleVector<Float, FloatVector>();
	CheckSimpleVector<size_t, SizeVector>();

	size_t  i, j;          // some temporary indices
	const Float zero( 0 ); // the value zero as a Float object
	size_t  imax;          // row index of maximum element
	size_t  jmax;          // column indx of maximum element
	Float    emax;         // maximum absolute value
	size_t  p;             // count pivots
	int     sign;          // sign of the permutation
	Float   etmp;          // temporary element
	Float   pivot;         // pivot element

	// -------------------------------------------------------
	size_t n = size_t(ip.size());
	CPPAD_ASSERT_KNOWN(
		size_t(jp.size()) == n,
		"Error in LuFactor: jp must have size equal to n"
	);
	CPPAD_ASSERT_KNOWN(
		size_t(LU.size()) == n * n,
		"Error in LuFactor: LU must have size equal to n * m"
	);
	// -------------------------------------------------------

	// initialize row and column order in matrix not yet pivoted
	for(i = 0; i < n; i++)
	{	ip[i] = i;
		jp[i] = i;
	}
	// initialize the sign of the permutation
	sign = 1;
	// initialize the ratio                                             //
	ratio = Float(1);                                                   //
	// ---------------------------------------------------------

	// Reduce the matrix P to L * U using n pivots
	for(p = 0; p < n; p++)
	{	// determine row and column corresponding to element of
		// maximum absolute value in remaining part of P
		imax = jmax = n;
		emax = zero;
		for(i = p; i < n; i++)
		{	for(j = p; j < n; j++)
			{	CPPAD_ASSERT_UNKNOWN(
					(ip[i] < n) & (jp[j] < n)
				);
				etmp = LU[ ip[i] * n + jp[j] ];

				// check if maximum absolute value so far
				if( AbsGeq (etmp, emax) )
				{	imax = i;
					jmax = j;
					emax = etmp;
				}
			}
		}
		for(i = p; i < n; i++)                                       //
		{	for(j = p; j < n; j++)                               //
			{	etmp  = abs(LU[ ip[i] * n + jp[j] ] / emax); //
				ratio =                                      //
				CondExpGt(etmp, ratio, etmp, ratio);         //
			}                                                    //
		}                                                            //
		CPPAD_ASSERT_KNOWN(
			(imax < n) & (jmax < n) ,
			"AbsGeq must return true when second argument is zero"
		);
		if( imax != p )
		{	// switch rows so max absolute element is in row p
			i        = ip[p];
			ip[p]    = ip[imax];
			ip[imax] = i;
			sign     = -sign;
		}
		if( jmax != p )
		{	// switch columns so max absolute element is in column p
			j        = jp[p];
			jp[p]    = jp[jmax];
			jp[jmax] = j;
			sign     = -sign;
		}
		// pivot using the max absolute element
		pivot   = LU[ ip[p] * n + jp[p] ];

		// check for determinant equal to zero
		if( pivot == zero )
		{	// abort the mission
			return   0;
		}

		// Reduce U by the elementary transformations that maps
		// LU( ip[p], jp[p] ) to one.  Only need transform elements
		// above the diagonal in U and LU( ip[p] , jp[p] ) is
		// corresponding value below diagonal in L.
		for(j = p+1; j < n; j++)
			LU[ ip[p] * n + jp[j] ] /= pivot;

		// Reduce U by the elementary transformations that maps
		// LU( ip[i], jp[p] ) to zero. Only need transform elements
		// above the diagonal in U and LU( ip[i], jp[p] ) is
		// corresponding value below diagonal in L.
		for(i = p+1; i < n; i++ )
		{	etmp = LU[ ip[i] * n + jp[p] ];
			for(j = p+1; j < n; j++)
			{	LU[ ip[i] * n + jp[j] ] -=
					etmp * LU[ ip[p] * n + jp[j] ];
			}
		}
	}
	return sign;
}
Example #20
0
bool
Aggregate::onLiteralFalse(
    Solver& solver,
    Literal currentLiteral,
    PropagatorData p )
{
    int position = p.position();
    assert_msg( abs( position ) > 0 && abs( position ) < static_cast< int >( literals.size() ), abs( position ) << " >= " << literals.size() );
    assert_msg( currentLiteral == ( position < 0 ? literals[ -position ].getOppositeLiteral() : literals[ position ] ), currentLiteral << " != " << ( position < 0 ? literals[ -position ].getOppositeLiteral() : literals[ position ] ) );
    trace_msg( aggregates, 10, "Aggregate: " << *this << ". Literal: " << currentLiteral.getOppositeLiteral() << " is true. Position: " << position );
    int ac = ( position < 0 ? POS : NEG );
    Literal aggrLiteral = ( ac == POS ? literals[ 1 ].getOppositeLiteral() : literals[ 1 ] );
    
    if( solver.isTrue( aggrLiteral ) || active + ac == 0 )
    {        
        trace_msg( aggregates, 2, "Return. AggrLiteral: " << aggrLiteral << " - Active: " << active << " - Ac: " << ac );
        return false;
    }
    
    unsigned int index = ( position > 0 ? position : -position ); 
    int64_t& counter = ( position > 0 ? counterW2 : counterW1 );

    trace_msg( aggregates, 2, "Updating counter. Old value: " << counter << " - New value: " << counter - weights[ index ] );
    
    if( counter < ( int64_t ) weights[ index ] )
    {
        assert_msg( solver.getDecisionLevel( currentLiteral ) == 0, "Literal " << currentLiteral << " in " << *this << " has a decision level " << solver.getDecisionLevel( currentLiteral ) );
        trace_msg( aggregates, 3, "A conflict happened." );        
        solver.assignLiteral( currentLiteral, this );
        return false;
    }
    assert( counter >= ( int64_t ) weights[ index ] );
    counter -= weights[ index ];
    watched[ index ] = false;
    
    if( solver.getDecisionLevel( currentLiteral ) != 0 )
        trail.push_back( position );

    trace_msg( aggregates, 2, "Umax: " << umax << " - size: " << size() );
    while( umax < literals.size() && ( int64_t ) weights[ umax ] > counter )
    {
        if( watched[ umax ] )
        {
            if( literalOfUnroll == Literal::null )
                literalOfUnroll = currentLiteral;
            active = ac;
            Literal lit = ( ac == POS ? literals[ umax ].getOppositeLiteral() : literals[ umax ] );
            if( !solver.isTrue( lit ) )
            {                
                //Maybe we don't need to add the position of this literal
                trail.push_back( umax * ac );
            
                trace_msg( aggregates, 9, "Inferring " << lit << " as true" );
//                createClauseFromTrail( lit );
                solver.assignLiteral( lit, this );
                if( solver.conflictDetected() )
                    return true;
            }
            else
            {
                trace_msg( aggregates, 9, "Skipping true literal " << lit );
            }
        }
        
        ++umax;
        trace_msg( aggregates, 3, "Updated umax. New Value: " << umax );        
    }
    
    return true;
}
Example #21
0
bool obs_vector_load_from_HISTORY_OBSERVATION(obs_vector_type * obs_vector ,
                                              const conf_instance_type * conf_instance ,
                                              time_map_type * obs_time ,
                                              const history_type * history ,
                                              ensemble_config_type * ensemble_config,
                                              double std_cutoff ) {

  if(!conf_instance_is_of_class(conf_instance, "HISTORY_OBSERVATION"))
    util_abort("%s: internal error. expected \"HISTORY_OBSERVATION\" instance, got \"%s\".\n",__func__, conf_instance_get_class_name_ref(conf_instance) );

  {
    bool initOK = false;
    int          size , restart_nr;
    double_vector_type * value              = double_vector_alloc(0,0);
    double_vector_type * std                = double_vector_alloc(0,0);
    bool_vector_type   * valid              = bool_vector_alloc(0 , false);

    /* The auto_corrf parameters can not be "segmentized" */
    double auto_corrf_param                 = -1;
    const char * auto_corrf_name            = NULL;


    double         error      = conf_instance_get_item_value_double(conf_instance, "ERROR"     );
    double         error_min  = conf_instance_get_item_value_double(conf_instance, "ERROR_MIN" );
    const char *   error_mode = conf_instance_get_item_value_ref(   conf_instance, "ERROR_MODE");
    const char *   sum_key    = conf_instance_get_name_ref(         conf_instance              );

    if(conf_instance_has_item(conf_instance, "AUTO_CORRF")) {
      auto_corrf_name  = conf_instance_get_item_value_ref( conf_instance , "AUTO_CORRF");
      auto_corrf_param = conf_instance_get_item_value_double(conf_instance, "AUTO_CORRF_PARAM");
      if(conf_instance_has_item(conf_instance, "AUTO_CORRF_PARAM"))
        auto_corrf_param = conf_instance_get_item_value_double(conf_instance, "AUTO_CORRF_PARAM");
      else
        util_abort("%s: When specifying AUTO_CORRF you must also give a vlaue for AUTO_CORRF_PARAM",__func__);
    }


    // Get time series data from history object and allocate
    size = time_map_get_last_step( obs_time );
    if (history_init_ts( history , sum_key , value , valid )) {

      // Create  the standard deviation vector
      if(strcmp(error_mode, "ABS") == 0) {
        for( restart_nr = 0; restart_nr < size; restart_nr++)
          double_vector_iset( std , restart_nr , error );
      } else if(strcmp(error_mode, "REL") == 0) {
        for( restart_nr = 0; restart_nr < size; restart_nr++)
          double_vector_iset( std , restart_nr , error * abs( double_vector_iget( value , restart_nr )));
      } else if(strcmp(error_mode, "RELMIN") == 0) {
        for(restart_nr = 0; restart_nr < size; restart_nr++) {
          double tmp_std = util_double_max( error_min , error * abs( double_vector_iget( value , restart_nr )));
          double_vector_iset( std , restart_nr , tmp_std);
        }
      } else
        util_abort("%s: Internal error. Unknown error mode \"%s\"\n", __func__, error_mode);


      // Handle SEGMENTs which can be used to customize the observation error. */
      {
        stringlist_type * segment_keys = conf_instance_alloc_list_of_sub_instances_of_class_by_name(conf_instance, "SEGMENT");
        stringlist_sort( segment_keys , NULL );

        int num_segments = stringlist_get_size(segment_keys);

        for(int segment_nr = 0; segment_nr < num_segments; segment_nr++)
          {
            const char * segment_name = stringlist_iget(segment_keys, segment_nr);
            const conf_instance_type * segment_conf = conf_instance_get_sub_instance_ref(conf_instance, segment_name);

            int start                         = conf_instance_get_item_value_int(   segment_conf, "START"     );
            int stop                          = conf_instance_get_item_value_int(   segment_conf, "STOP"      );
            double         error_segment      = conf_instance_get_item_value_double(segment_conf, "ERROR"     );
            double         error_min_segment  = conf_instance_get_item_value_double(segment_conf, "ERROR_MIN" );
            const char *   error_mode_segment = conf_instance_get_item_value_ref(   segment_conf, "ERROR_MODE");

            if(start < 0)
              {
                printf("%s: WARNING - Segment out of bounds. Truncating start of segment to 0.\n", __func__);
                start = 0;
              }

            if(stop >= size)
              {
                printf("%s: WARNING - Segment out of bounds. Truncating end of segment to %d.\n", __func__, size - 1);
                stop = size -1;
              }

            if(start > stop)
              {
                printf("%s: WARNING - Segment start after stop. Truncating end of segment to %d.\n", __func__, start );
                stop = start;
              }

            // Create  the standard deviation vector
            if(strcmp(error_mode_segment, "ABS") == 0) {
              for( restart_nr = start; restart_nr <= stop; restart_nr++)
                double_vector_iset( std , restart_nr , error_segment) ;
            } else if(strcmp(error_mode_segment, "REL") == 0) {
              for( restart_nr = start; restart_nr <= stop; restart_nr++)
                double_vector_iset( std , restart_nr , error_segment * abs(double_vector_iget( value , restart_nr)));
            } else if(strcmp(error_mode_segment, "RELMIN") == 0) {
              for(restart_nr = start; restart_nr <= stop ; restart_nr++) {
                double tmp_std = util_double_max( error_min_segment , error_segment * abs( double_vector_iget( value , restart_nr )));
                double_vector_iset( std , restart_nr , tmp_std);
              }
            } else
              util_abort("%s: Internal error. Unknown error mode \"%s\"\n", __func__, error_mode);
          }
        stringlist_free(segment_keys);
      }


      /*
        This is where the summary observations are finally added.
      */
      for (restart_nr = 0; restart_nr < size; restart_nr++) {
        if (bool_vector_safe_iget( valid , restart_nr)) {
          if (double_vector_iget( std , restart_nr) > std_cutoff) {
            obs_vector_add_summary_obs( obs_vector , restart_nr , sum_key , sum_key ,
                                        double_vector_iget( value ,restart_nr) , double_vector_iget( std , restart_nr ) ,
                                        auto_corrf_name , auto_corrf_param);
          } else
            fprintf(stderr,"** Warning: to small observation error in observation %s:%d - ignored. \n", sum_key , restart_nr);
        }
      }
      initOK = true;
    }
    double_vector_free(std);
    double_vector_free(value);
    bool_vector_free(valid);
    return initOK;
  }
}
bool VACVariable::averaging()
{
	Cost Top = wcsp->getUb();
	bool change = false;
	EnumeratedVariable* x;
	EnumeratedVariable* y;
	Constraint* ctr = NULL;
	ConstraintList::iterator itc = getConstrs()->begin();
	if(itc != getConstrs()->end())	ctr = (*itc).constr;
	while(ctr) {
		if(ctr->arity() == 2 && !ctr->isSep()) {
			BinaryConstraint* bctr = (BinaryConstraint*) ctr;
			x = (EnumeratedVariable*) bctr->getVarDiffFrom( (Variable*) this );
			for (iterator it = begin(); it != end(); ++it) {
				Cost cu = getCost(*it);
				Cost cmin = Top;
				for (iterator itx = x->begin(); itx != x->end(); ++itx) {
					Cost cbin = bctr->getCost(this,x,*it,*itx);
					if(cbin < cmin) cmin = cbin;
				}
				assert(cmin < Top);
				Double mean = to_double(cmin + cu) / 2.;	
				Double extc = to_double(cu) - mean;					 
				if(abs(extc) >= 1) {
				  Cost costi = (Long) extc;
				  for (iterator itx = x->begin(); itx != x->end(); ++itx) {
					bctr->addcost(this,x,*it,*itx,costi);				
				  }
				  if(mean > to_double(cu)) project(*it, -costi); 
				  else extend(*it, costi);
				  change = true;
				}
			}
		} else if(ctr->arity() == 3 && !ctr->isSep()) {
			TernaryConstraint* tctr = (TernaryConstraint*) ctr;
			x = (EnumeratedVariable*) tctr->getVar( 0 );
			if(x == this) x = (EnumeratedVariable*) tctr->getVar( 1 );
		    y = (EnumeratedVariable*) tctr->getVarDiffFrom((Variable*) this, (Variable*)x);
			for (iterator it = begin(); it != end(); ++it) {
				Cost cu = getCost(*it);
				Cost cmin = Top;
				for (iterator itx = x->begin(); itx != x->end(); ++itx) {
				for (iterator ity = y->begin(); ity != y->end(); ++ity) {
					Cost ctern = tctr->getCost(this,x,y,*it,*itx,*ity);
					if(ctern < cmin) cmin = ctern;
				}}
				assert(cmin < Top);
				Double mean = to_double(cmin + cu) / 2.;
				Double extc = to_double(cu) - mean;				 
				if(abs(extc) >= 1) {
					Cost costi = (Long) extc;
					for (iterator itx = x->begin(); itx != x->end(); ++itx) {
					for (iterator ity = y->begin(); ity != y->end(); ++ity) {
						tctr->addCost(this,x,y,*it,*itx,*ity,costi);				
					}}
					if(mean > to_double(cu)) project(*it, -costi); 
					else extend(*it, costi);
					change = true;
				}
			}
		} else if(ctr->arity() >= 4 && ctr->extension() && !ctr->isSep()) {
			NaryConstraint* nctr = (NaryConstraint*) ctr;
			for (iterator it = begin(); it != end(); ++it) {
				Cost cu = getCost(*it);
				Cost cmin = Top;
				int tindex = nctr->getIndex(this);
				String tuple;
				Cost cost;
				Long nbtuples = 0;
				nctr->first();
				while (nctr->next(tuple,cost)) {
				  nbtuples++;
				  if (toValue(tuple[tindex] - CHAR_FIRST)==(*it) && cost < cmin) cmin = cost;
				}
				if (nctr->getDefCost() < cmin && nbtuples < nctr->getDomainSizeProduct()) cmin = nctr->getDefCost();
				//				assert(cmin < Top);
				Double mean = to_double(cmin + cu) / 2.;
				Double extc = to_double(cu) - mean;				 
				if(abs(extc) >= 1) {
					Cost costi = (Cost) extc;
					if(nctr->getDefCost() < Top) {
					  nctr->firstlex();
					  while( nctr->nextlex(tuple,cost) ) {
						if (toValue(tuple[tindex] - CHAR_FIRST)==(*it)) {
						  if(cost + costi < Top) nctr->setTuple(tuple, cost + costi);
						  else nctr->setTuple(tuple, Top);
						}
					  }
					  nctr->setDefCost(Top);
					} else {
					  nctr->first();
					  while( nctr->next(tuple,cost) ) {
						if (toValue(tuple[tindex] - CHAR_FIRST)==(*it)) {
						  if(cost + costi < Top) nctr->addtoTuple(tuple, costi);
						  else nctr->setTuple(tuple, Top);
						}
					  }
					}
					if(mean > to_double(cu)) project(*it, -costi); 
					else extend(*it, costi);
					change = true;
				}
			}
		}
		++itc;
		if(itc != getConstrs()->end()) ctr = (*itc).constr;
		else ctr = NULL;
	}
	return change;
}
Example #23
0
int
power_status(int fd, int force, struct apm_power_info *pinfo)
{
	struct apm_power_info bstate;
	static struct apm_power_info last;
	int acon = 0;

	if (fd == -1) {
		if (pinfo) {
			bstate.battery_state = 255;
			bstate.ac_state = 255;
			bstate.battery_life = 0;
			bstate.minutes_left = -1;
			*pinfo = bstate;
		}

		return 0;
	}

	if (ioctl(fd, APM_IOC_GETPOWER, &bstate) == 0) {
	/* various conditions under which we report status:  something changed
	 * enough since last report, or asked to force a print */
		if (bstate.ac_state == APM_AC_ON)
			acon = 1;
		if (force ||
		    bstate.ac_state != last.ac_state ||
		    bstate.battery_state != last.battery_state ||
		    (bstate.minutes_left && bstate.minutes_left < 15) ||
		    abs(bstate.battery_life - last.battery_life) > 20) {
#ifdef __powerpc__
			/*
			 * When the battery is charging, the estimated life
			 * time is in fact the estimated remaining charge time
			 * on Apple machines, so lie in the stats.
			 * We still want an useful message if the battery or
			 * ac status changes, however.
			 */
			if (bstate.minutes_left != 0 &&
			    bstate.battery_state != APM_BATT_CHARGING)
#else
			if ((int)bstate.minutes_left > 0)
#endif
				syslog(LOG_NOTICE, "battery status: %s. "
				    "external power status: %s. "
				    "estimated battery life %d%% (%u minutes)",
				    battstate(bstate.battery_state),
				    ac_state(bstate.ac_state),
				    bstate.battery_life,
				    bstate.minutes_left);
			else
				syslog(LOG_NOTICE, "battery status: %s. "
				    "external power status: %s. "
				    "estimated battery life %d%%",
				    battstate(bstate.battery_state),
				    ac_state(bstate.ac_state),
				    bstate.battery_life);
			last = bstate;
		}
		if (pinfo)
			*pinfo = bstate;
	} else
		syslog(LOG_ERR, "cannot fetch power status: %m");

	return acon;
}
Example #24
0
/*========================================================= 
Name: read_temp
Description:
This thread reads temperature from the thermistor through
the ADC. It converts the ADC counts to temperature and
calculates the PWM duty demand for the fan.
*=========================================================*/
void * read_temp(void * arg)
{
unsigned short int thermistor_counts; /* ADC value in counts */
unsigned short int thermistor_counts_prev; /* Previous value of ADC counts */
float temp_adc_v; /* ADC value in volts */
float temp; /* Temperature read by the thermistor */
float temp_ramp = 0.0f; /* Temperature ramped output */
static float prev_temp_ramp; /* Previous value of temperature ramped output  */
static float pwm_temp = 0.0f; /* Value of temperature when PWM was updated */
unsigned char index = 0u;
static unsigned char first_run = TRUE; /* Flag to indicate first execution */
unsigned char print = FALSE; /* Display temperature */

/* Run indefinitely */
while(1)
{
	/* Transmit ADC read command and receive results */
	thermistor_counts = spi_read_adc(0);

	/* Ignore small change in counts */
	if (abs(thermistor_counts_prev - 
		thermistor_counts) < ADC_DIFFERENCE)
	{
		thermistor_counts = thermistor_counts_prev;
	}
	
	/* Convert counts to volts */
	temp_adc_v = thermistor_counts * ADC_SCALING;

	/* Convert volts to temperature */				
	temp = get_temperature(temp_adc_v);
		
	/* First run */
	if (TRUE == first_run)
	{
		first_run = FALSE;
		
		/* No need to ramp for the first time */
		temp_ramp = temp;
		print = TRUE;
	}	
	
	/* Look for temperature change and ramp slowly */
	if (calc_diff(temp_ramp, temp) > 0.75)
	{
		temp_ramp = ramp_temperature(temp_ramp, temp);
		print = TRUE;
	}
	
	/* Display temperature */
	if (TRUE == print)
	{	
		print = FALSE;
		system("date");
		fprintf (stdout, "Temperature is: %4.2f degC\n", temp_ramp);
		
		/* Limit fan speed */
		if ((temp_ramp/3) < 0)
		{
			index = 0;
		}
		else if ((temp_ramp/3) > 10)
		{
			index = 10;
		}
		else
		{
			index = (temp_ramp/3);
		}
		
		/* Update PWM demand for temperature change more than 2 degrees */
		if (fabsf(temp_ramp - pwm_temp) >= 2.0)
		{
			pwm_temp = temp_ramp;
			pwm_demand = fan_speed_dc[index - 1];
			fprintf(stdout, "pwm demand %d%\n", pwm_demand);
		}
	}
	
	/* Wait before next read */	
	sleep(1);

	/* Store previous temperature value */
	prev_temp_ramp = temp_ramp;
	thermistor_counts_prev = thermistor_counts;
}
}
Example #25
0
CCTexture2D* TextureHelper::addImageFromPlist(const char* plist)
{
	std::string fullPath = CCFileUtils::sharedFileUtils()->fullPathForFilename(plist);
	CCDictionary *dict = CCDictionary::createWithContentsOfFileThreadSafe(fullPath.c_str());
	CCDictionary *metadataDict = (CCDictionary*)dict->objectForKey("metadata");
	std::string pngPath = metadataDict->valueForKey("realTextureFileName")->getCString();
	CCTexture2D* texture = CCTextureCache::sharedTextureCache()->addImage(pngPath.c_str());
	CCDictionary *framesDict = (CCDictionary*)dict->objectForKey("frames");
	int format = 0;
	CCSize textureSize = texture->getContentSize();

	if(metadataDict != NULL) 
	{
		format = metadataDict->valueForKey("format")->intValue();
	}

	CCAssert(format >=0 && format <= 3, "format is not supported for CCSpriteFrameCache addSpriteFramesWithDictionary:textureFilename:");

	CCDictElement* pElement = NULL;
	CCDICT_FOREACH(framesDict, pElement)
	{
		CCRect rect;
		CCDictionary* frameDict = (CCDictionary*)pElement->getObject();
		std::string spriteFrameName = pElement->getStrKey();
	
		if(format == 0) 
		{
			float x = frameDict->valueForKey("x")->floatValue();
			float y = frameDict->valueForKey("y")->floatValue();
			float w = frameDict->valueForKey("width")->floatValue();
			float h = frameDict->valueForKey("height")->floatValue();
			float ox = frameDict->valueForKey("offsetX")->floatValue();
			float oy = frameDict->valueForKey("offsetY")->floatValue();
			int ow = frameDict->valueForKey("originalWidth")->intValue();
			int oh = frameDict->valueForKey("originalHeight")->intValue();
			if(!ow || !oh)
			{
				CCLOGWARN("cocos2d: WARNING: originalWidth/Height not found on the CCSpriteFrame. AnchorPoint won't work as expected. Regenrate the .plist");
			}
			ow = abs(ow);
			oh = abs(oh);
			rect = CCRectMake(x + ox,textureSize.height - y + oy,ow,oh);
		 } 
		 else if(format == 1 || format == 2) 
		 {
			  CCRect frame = CCRectFromString(frameDict->valueForKey("frame")->getCString());
			  frame.origin.y = textureSize.height - frame.origin.y;
			  bool rotated = false;

			  if (format == 2)
			  {
				  rotated = frameDict->valueForKey("rotated")->boolValue();
			  }

			  CCPoint offset = CCPointFromString(frameDict->valueForKey("offset")->getCString());
			  CCSize sourceSize = CCSizeFromString(frameDict->valueForKey("sourceSize")->getCString());
			  rect = frame;
		  } 
		  else if (format == 3)
		  {
			  CCSize spriteSize = CCSizeFromString(frameDict->valueForKey("spriteSize")->getCString());
			  CCPoint spriteOffset = CCPointFromString(frameDict->valueForKey("spriteOffset")->getCString());
			  CCSize spriteSourceSize = CCSizeFromString(frameDict->valueForKey("spriteSourceSize")->getCString());
			  CCRect textureRect = CCRectFromString(frameDict->valueForKey("textureRect")->getCString());
			  bool textureRotated = frameDict->valueForKey("textureRotated")->boolValue();
			  rect = textureRect;
		  }
		 rectMap.insert(std::pair<std::string,CCRect>(spriteFrameName,rect));
	}
Example #26
0
/*
 * Capacity Learning Routine. The FG records a capacity that was put in, or
 * taken out of the battery on both Charge or Discharge. If, at the end of
 * charge or discharge, the gauge decides to update FCC, based on the learned
 * capacity amount, the gaue calls this function.
 */
void fg_learn_capacity(struct cell_state *cell, unsigned short capacity)
{
	short learn_limit;

	/* Make sure no learning is still in progress */
	cell->vcq = false;
	cell->vdq = false;

	/* Check if Learning needs to be Disqualified */
	//if (cell->temperature < cell->config->low_temp) {
		//dev_dbg(cell->dev, "LRN: Learning disqual (Temp) %d < %d\n",
			//cell->temperature,
			//cell->config->low_temp);
		//return;
	//}

	if (abs(cell->ocv_total_q) > cell->config->ocv->max_ocv_discharge) {
		dev_dbg(cell->dev, "LRN: Learning disqual (OCV) %d > %d\n",
			cell->ocv_total_q,
			cell->config->ocv->max_ocv_discharge);
		return;
	}

	dev_dbg(cell->dev, "LRN: Learn Capacity = %dmAh\n", capacity);

	/* Learned Capacity is much lower, then expected */
	learn_limit = cell->fcc - cell->config->max_decrement;
	if ((short) capacity < learn_limit) {
		capacity = learn_limit;
		dev_dbg(cell->dev, "LRN: Capacity is LOW, limit to %4dmAh\n",
			capacity);
	} else {
		learn_limit = cell->fcc + cell->config->max_increment;
		if ((short) capacity > learn_limit) {
			/* Learned Capacity is much greater, then expected */
			capacity = learn_limit;
			dev_dbg(cell->dev,
				"LRN: Capacity is HIGH, limit to %4dmAh\n",
				capacity);
		} else {
			/* capacity within expected range */
			dev_dbg(cell->dev,
				"LRN: Capacity within range (%d < %d < %d)\n",
				cell->fcc - cell->config->max_decrement,
				capacity,
				cell->fcc + cell->config->max_increment);
		}
	}

	/* Reset No Learn Counter */
	cell->learned_cycle = cell->cycle_count;

	cell->new_fcc = capacity;

	/* We can update FCC here, only if charging is on */
	if (!cell->edv2) {
		dev_dbg(cell->dev,
			"LRN: FCC Updated, newFCC = %d, FCC = %d mAh\n",
			cell->new_fcc,
			cell->fcc);

		cell->fcc = cell->new_fcc;
		cell->updated = true;
	} else {
		dev_dbg(cell->dev,
			"LRN: FCC <- %dmAh, on next CHG\n", cell->new_fcc);
	}

}
Example #27
0
void debut_jeu(grille *g, grille *gc){
	char c = getchar(); 
    while (c != 'q') // touche 'q' pour quitter
    {
        printf("\e[J");
		switch (c) {
			case '\n' : 
            { // touche "entree" pour évoluer
				evolue(g,gc);
				efface_grille(*g);
				affiche_grille(*g);
				break;
			}
            case 'c' : // touche pour changer de mode cyclique/non-cyclique
            {
				cyclique=abs(cyclique-1);
                compte_voisins_vivants=(cyclique==0) ? compte_cyclique : compte_non_cyclique;
                printf("\n\e[1A");
				break;
            }
            case 'v' : // touche pour activer/désactiver le veillissement
            {
                viellissement=abs(viellissement-1);
                if (!viellissement)
                { //effacer les âges d'avant
                    int i,j;
                    for (i=0; i<g->nbl; i++)
                       for (j=0;j<g->nbc; j++)
                           if (est_vivante(i,j,*g)) set_vivante(i,j,*g);
                           else set_morte(i,j,*g);
                }
                printf("\n\e[1A");
				break;
			}
            case 'o' :  // touche pour tester si la colonie est oscillante
            {
                int periode=0, failsafe=0;
                grille gtest,gmorte; //g doit rester inchangée
                alloue_grille (g->nbl, g->nbc, &gtest);
                alloue_grille (g->nbl, g->nbc, &gmorte);
                copie_grille(*g,gtest);
                do {
                    periode++;
                    failsafe++;
                    evolue(&gtest,gc);
                } while (!( compare_grilles(gtest,*g) || compare_grilles(gtest,gmorte) )
                          && failsafe < MAX_TEST );
                //soit on retombe sur la même configuration (-> oscillant)
                //soit toutes les cellules seront mortes à un moment (-> pas oscillant)

                if (compare_grilles(gtest,gmorte) || failsafe==MAX_TEST)
                    printf("Pas oscillant.");
                else
                    printf("Oscillant. Période: %d ",periode);
                libere_grille(&gtest);
                libere_grille(&gmorte);
                break;
            }
			default : 
            { // touche non traitée
				printf("\n\e[1A");
				break;
			}
        }
		c = getchar(); 
	}
	return;	
}
Example #28
0
/* Main FG entry point. This function needs to be called periodically.*/
void fg_process(struct cell_state *cell, short delta_q, short raw_voltage, short norm_voltage,
		short cur, short temperature)
{
	int i, tmp;
	struct timespec now;

	if (!cell->init)
		return;

	/* Update voltage and add it to the buffer, update average*/
	tmp = 0;
	cell->voltage = norm_voltage;
	av_v_index++;
	av_v_index %= AV_SIZE;
	av_v[av_v_index] = norm_voltage;
	for (i = 0; i < AV_SIZE; i++) {
		tmp += av_v[i];
	}
	cell->av_voltage = tmp/AV_SIZE;

	/* Update current and add it to the buffer, update average*/
	tmp = 0;
	cell->cur = cur;
	av_c_index++;
	av_c_index %= AV_SIZE;
	av_c[av_c_index] = cur;
	for (i = 0; i < AV_SIZE; i++)
		tmp += av_c[i];
	cell->av_current = tmp/AV_SIZE;

	/* Update temperature*/
	cell->temperature = temperature;
	/*Update FCC and EDV table from temperature compensation*/
	fg_temperature_compensate(cell);
	/* Check time since last_call */
	getrawmonotonic(&now);
	tmp = now.tv_sec - cell->last_correction.tv_sec;

	/* Check what capacity correction algorithm should we use: OCV or CC */
	if ((tmp > cell->config->ocv->relax_period) &&
	    (abs(cell->cur) < cell->config->ocv->long_sleep_current)) {
#ifdef CONFIG_POWER_SUPPLY_DEBUG
			printk(KERN_DEBUG "OCV check 1\n");
#endif
			fg_ocv(cell);
	} else if (fg_check_relaxed(cell)) {
		/* We are not doing any active CHG/DSG, clear flags
		   this does not compromise learning cycles */
		cell->chg = false;
		cell->dsg = false;

		/* Checking if we can do an OCV correction */
		if (fg_can_ocv(cell))
			fg_ocv(cell);
		else
			fg_cc(cell, delta_q);
	} else /* Not Relaxed: actively charging or discharging */
		fg_cc(cell, delta_q);


	/* Charge / Discharge spesific functionality */
	if (!cell->sleep) {
		if (cell->cur > 0)
			fg_charging(cell, delta_q);
		else if (cell->cur < 0)
			fg_discharging(cell, delta_q);
	}

	/* Update Regular SOC */
	cell->soc = DIV_ROUND_CLOSEST(cell->nac * MAX_PERCENTAGE, cell->fcc);

/* The voltage will vibrate a lot when close to shutdown voltage. This happens especially when running high power
 * consumption app. So need to take care of the end check
*/

	/*Add here to check both nac and EDV0*/
	if(cell->soc > EDV_FIRST_CHECK_POINT+5){
		fg_ocv_check(cell);
	}
	else if(cell->soc==0){
#ifdef BATTERY_DRAIN_CONFIG
		cell->soc=1;
#else
		fg_check_end(cell);
#endif
	}

	/* When SOC is below 15%, check if raw voltage level reached hardware shutdown threshold */
	if(cell->soc < EDV_FIRST_CHECK_POINT+5){
		fg_check_shutdown_voltage(cell, raw_voltage);
	}

	fg_update_edv_flags(cell, false);

	/* Check if battery is full */
	if (cell->cc) {
		cell->full = true;
	}

	if (cell->soc < MAX_PERCENTAGE) {
		cell->full = false;
		if (cell->nac <= (cell->fcc - cell->config->recharge))
			cell->cc = false;
	}

	/* Check if SOC reached 100% and battery is still charging, then keep SOC at 99% */
	if((*cell->charge_status == POWER_SUPPLY_STATUS_CHARGING) &&
			(cell->soc == MAX_PERCENTAGE) && !cell->cc && !cell->full)
	{
#ifdef DEBUG
		printk(KERN_DEBUG "End of charge not reach yet\n");
#endif
		cell->soc = MAX_PERCENTAGE-1;
	}

	/* Checking if we need to set an updated flag (is SOC changed) */
	if (cell->prev_soc != cell->soc) {
		cell->prev_soc = cell->soc;
		cell->updated = true;
	}

	cell->last_correction.tv_sec = now.tv_sec;

#if 0
	//JET-376 test, run calibration every 5-min to check cc_offset value
	cal_cntr++;
	if(cal_cntr >= 30)
	{
		cell->calibrate = true;
		cal_cntr = 0;
		printk(KERN_DEBUG "PMIC Calibrate now! \n");
	}
#endif

#ifdef DEBUG
	/* Printing Debug Data */
	printk(KERN_DEBUG
		"FG:ACC:%2d; RM:%4d,mAh; SOC:%3d%%; VOLT:%4d,%4d,mV; "
		"CUR:%5d,%5d,mA; TEMP:%3d; "
		"EDV:%d,mV,%02d%%; "
		"LLL:%4d,%4d,%4d; "
		"N/O:%4d,%4d; "
		"CS:%4d; EL:%5d; "
		"FCC:%d,%d; CP:%d; \n",
		delta_q, cell->nac, cell->soc, cell->voltage, cell->av_voltage,
		cell->cur, cell->av_current, cell->temperature,
		cell->edv.voltage, cell->edv.percent,
		cell->learn_q, cell->learn_offset, cell->ocv_total_q,
		cell->negative_q, cell->overcharge_q,
		cell->cumulative_sleep, cell->electronics_load,
		cell->fcc, cell->cycle_count, tmp);

	print_flags(cell);
#endif
}
task main()
{
	waitForStart();
	while(true)
	{

		getJoystickSettings(joystick);
//first joypad
	//right drive
	 const int threshold =8; // Int 'threshold' will allow us to ignore low  readings that keep our robot in perpetual motion.
		if(abs(joystick.joy1_y1) > threshold)   // If the right analog stick's Y-axis readings are either above or below the threshold:
		{
		motor[RightMotor] = joystick.joy1_y1;         // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
		}else                                    // Else if the readings are within the threshold:
			{
				motor[RightMotor] = 0;                        // Motor D is stopped with a power level of 0.
			}
 //left drive
		if(abs(joystick.joy1_y2) > threshold)   // If the left analog stick's Y-axis readings are either above or below the threshold:
		{
		motor[LeftMotor] = joystick.joy1_y2;         // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
		}
			else                                    // Else if the readings are within the threshold:
			{
				motor[LeftMotor] = 0;                        // Motor E is stopped with a power level of 0.
			}
//Flag Raiser
			if(joy1Btn(5))
			{
				motor[FlagRaiser] = 80;
		  }
		  else if(joy1Btn(7))
		  {
		  	motor[FlagRaiser] = -80;
		  }
		  else
		  {
		 		motor[FlagRaiser] = 0;
		  }
//Second Joypad
		//Tower Lift--has two paralell motors mounted on the uprights
		if(joy2Btn(2))//A
			{
				motor[TowerLift]=-80;//down
			}else if(joy2Btn(3))//B
				{
					motor[TowerLift]=80;//up
				}else//not A, or B...
					{
						motor[TowerLift]=0;//stop
					}
	//lift--has one motor mounted at the tower cross-section
		if(joy2Btn(4))//X
			{
				writeDebugStreamLine("arm: %i", nMotorEncoder[potentiometer]);
				motor[Lift]=80;//down
			}else if(joy2Btn(1))//Y
				{
					writeDebugStreamLine("arm: %i", nMotorEncoder[potentiometer]);
					motor[Lift]=-80;//up
				}else//not X, or Y...
					{
						motor[Lift]=0;//stop
					}
	//Block Gate
			if(joy2Btn(6))//LB
			{
			servo[BlockGate]=254;// opens the block gate
			}else//not LB...
				{
					servo[BlockGate]=5;//close block gate
				}
	//ejector
			if(joy2Btn(8)&&servo[BlockGate] !=5)
			{
				servo[Ejector]=100;//opens the ejector servo
			}else
				{
					servo[Ejector]=5;//closes the ejector servo
				}
	}//end of while(true)
}//main end block
Example #30
0
bool moveIsDP(Move move) {
	assert(moveIsValid(move));
	int toRank=sqRank(moveGetToSqRaw(move));
	int fromRank=sqRank(moveGetFromSq(move));
	return (moveGetToPieceType(move)==PieceTypePawn && abs(toRank-fromRank)==2);
}