void TCOD_map_compute_fov_restrictive_shadowcasting_quadrant (map_t *m, int player_x, int player_y, int max_radius, bool light_walls, int maxObstacles, int dx, int dy) { static double *startAngle=NULL, *endAngle=NULL; static int angleArraySize=0; if ( angleArraySize > 0 && angleArraySize < maxObstacles ) { free(startAngle); startAngle=NULL; } if ( startAngle == NULL ) { angleArraySize = maxObstacles; startAngle = (double *)malloc(sizeof(double) * 2 * maxObstacles); endAngle = &startAngle[maxObstacles]; } //octant: vertical edge { int iteration = 1; //iteration of the algo for this octant bool done = false; int totalObstacles = 0; int obstaclesInLastLine = 0; double minAngle = 0.0f; int x,y; //do while there are unblocked slopes left and the algo is within the map's boundaries //scan progressive lines/columns from the PC outwards y = player_y+dy; //the outer slope's coordinates (first processed line) if (y < 0 || y >= m->height) done = true; while(!done) { //process cells in the line double slopesPerCell = 1.0f/(double)(iteration+1); double halfSlopes = slopesPerCell*0.5f; int processedCell = (int)(minAngle / slopesPerCell); int minx = MAX(0,player_x-iteration), maxx = MIN(m->width-1,player_x+iteration); done = true; for (x = player_x + (processedCell * dx); x >= minx && x <= maxx; x+=dx) { int c = x + (y * m->width); //calculate slopes per cell bool visible = true; double startSlope = (double)processedCell*slopesPerCell; double centreSlope = startSlope+halfSlopes; double endSlope = startSlope+slopesPerCell; if (obstaclesInLastLine > 0 && m->cells[c].fov == 0) { int idx = 0; while(visible && idx < obstaclesInLastLine) { if (m->cells[c].transparent == true) { if (centreSlope > startAngle[idx] && centreSlope < endAngle[idx]) visible = false; } else { if (startSlope >= startAngle[idx] && endSlope <= endAngle[idx]) visible = false; } if (visible && (m->cells[c-(m->width*dy)].fov == 0 || !m->cells[c-(m->width*dy)].transparent) && (x-dx >= 0 && x-dx < m->width && (m->cells[c-(m->width*dy)-dx].fov == 0 || !m->cells[c-(m->width*dy)-dx].transparent))) visible = false; idx++; } } if (visible) { m->cells[c].fov = 1; done = false; //if the cell is opaque, block the adjacent slopes if (!m->cells[c].transparent) { if (minAngle >= startSlope) minAngle = endSlope; else { startAngle[totalObstacles] = startSlope; endAngle[totalObstacles++] = endSlope; } if (!light_walls) m->cells[c].fov = 0; } } processedCell++; } if (iteration == max_radius) done = true; iteration++; obstaclesInLastLine = totalObstacles; y += dy; if (y < 0 || y >= m->height) done = true; if ( minAngle == 1.0f ) done=true; } } //octant: horizontal edge { int iteration = 1; //iteration of the algo for this octant bool done = false; int totalObstacles = 0; int obstaclesInLastLine = 0; double minAngle = 0.0f; int x,y; //do while there are unblocked slopes left and the algo is within the map's boundaries //scan progressive lines/columns from the PC outwards x = player_x+dx; //the outer slope's coordinates (first processed line) if (x < 0 || x >= m->width) done = true; while(!done) { //process cells in the line double slopesPerCell = 1.0f/(double)(iteration+1); double halfSlopes = slopesPerCell*0.5f; int processedCell = (int)(minAngle / slopesPerCell); int miny = MAX(0,player_y-iteration), maxy = MIN(m->height-1,player_y+iteration); done = true; for (y = player_y + (processedCell * dy); y >= miny && y <= maxy; y+=dy) { int c = x + (y * m->width); //calculate slopes per cell bool visible = true; double startSlope = (double)processedCell*slopesPerCell; double centreSlope = startSlope+halfSlopes; double endSlope = startSlope+slopesPerCell; if (obstaclesInLastLine > 0 && m->cells[c].fov == 0) { int idx = 0; while(visible && idx < obstaclesInLastLine) { if (m->cells[c].transparent == true) { if (centreSlope > startAngle[idx] && centreSlope < endAngle[idx]) visible = false; } else { if (startSlope >= startAngle[idx] && endSlope <= endAngle[idx]) visible = false; } if (visible && (m->cells[c-dx].fov == 0 || !m->cells[c-dx].transparent) && (y-dy >= 0 && y-dy < m->height && (m->cells[c-(m->width*dy)-dx].fov == 0 || !m->cells[c-(m->width*dy)-dx].transparent))) visible = false; idx++; } } if (visible) { m->cells[c].fov = 1; done = false; //if the cell is opaque, block the adjacent slopes if (!m->cells[c].transparent) { if (minAngle >= startSlope) minAngle = endSlope; else { startAngle[totalObstacles] = startSlope; endAngle[totalObstacles++] = endSlope; } if (!light_walls) m->cells[c].fov = 0; } } processedCell++; } if (iteration == max_radius) done = true; iteration++; obstaclesInLastLine = totalObstacles; x += dx; if (x < 0 || x >= m->width) done = true; if ( minAngle == 1.0f ) done=true; } } }
int computeArea(int A, int B, int C, int D, int E, int F, int G, int H) { int i=MAX(A,E),j=MAX(B,F),m=MIN(C,G),n=MIN(D,H),area=(D-B)*(C-A)+(H-F)*(G-E); if(i<m && j<n) area-=(n-j)*(m-i); return area; }
/* * Set "p_ptr->food", notice observable changes * * The "p_ptr->food" variable can get as large as 20000, allowing the * addition of the most "filling" item, Elvish Waybread, which adds * 7500 food units, without overflowing the 32767 maximum limit. * * Perhaps we should disturb the player with various messages, * especially messages about hunger status changes. XXX XXX XXX * * Digestion of food is handled in "dungeon.c", in which, normally, * the player digests about 20 food units per 100 game turns, more * when "fast", more when "regenerating", less with "slow digestion", * but when the player is "gorged", he digests 100 food units per 10 * game turns, or a full 1000 food units per 100 game turns. * * Note that the player's speed is reduced by 10 units while gorged, * so if the player eats a single food ration (5000 food units) when * full (15000 food units), he will be gorged for (5000/100)*10 = 500 * game turns, or 500/(100/5) = 25 player turns (if nothing else is * affecting the player speed). */ bool set_food(int v) { int old_aux, new_aux; bool notice = FALSE; /* Hack -- Force good values */ v = MIN(v, PY_FOOD_UPPER); v = MAX(v, 0); /* Fainting / Starving */ if (p_ptr->food < PY_FOOD_FAINT) { old_aux = 0; } /* Weak */ else if (p_ptr->food < PY_FOOD_WEAK) { old_aux = 1; } /* Hungry */ else if (p_ptr->food < PY_FOOD_ALERT) { old_aux = 2; } /* Normal */ else if (p_ptr->food < PY_FOOD_FULL) { old_aux = 3; } /* Full */ else if (p_ptr->food < PY_FOOD_MAX) { old_aux = 4; } /* Gorged */ else { old_aux = 5; } /* Fainting / Starving */ if (v < PY_FOOD_FAINT) { new_aux = 0; } /* Weak */ else if (v < PY_FOOD_WEAK) { new_aux = 1; } /* Hungry */ else if (v < PY_FOOD_ALERT) { new_aux = 2; } /* Normal */ else if (v < PY_FOOD_FULL) { new_aux = 3; } /* Full */ else if (v < PY_FOOD_MAX) { new_aux = 4; } /* Gorged */ else { new_aux = 5; } /* Food increase */ if (new_aux > old_aux) { /* Describe the state */ switch (new_aux) { /* Weak */ case 1: { msg_print("You are still weak."); break; } /* Hungry */ case 2: { msg_print("You are still hungry."); break; } /* Normal */ case 3: { msg_print("You are no longer hungry."); break; } /* Full */ case 4: { msg_print("You are full!"); break; } /* Bloated */ case 5: { message(MSG_HUNGRY, 0, "You have gorged yourself!"); break; } } /* Change */ notice = TRUE; } /* Food decrease */ else if (new_aux < old_aux) { /* Describe the state */ switch (new_aux) { /* Fainting / Starving */ case 0: { sound(MSG_NOTICE); message(MSG_PARALYZED, 0, "You are getting faint from hunger!"); break; } /* Weak */ case 1: { sound(MSG_NOTICE); message(MSG_HUNGRY, 0, "You are getting weak from hunger!"); break; } /* Hungry */ case 2: { sound(MSG_HUNGRY); message(MSG_HUNGRY, 0, "You are getting hungry."); break; } /* Normal */ case 3: { sound(MSG_NOTICE); msg_print("You are no longer full."); break; } /* Full */ case 4: { sound(MSG_NOTICE); msg_print("You are no longer gorged."); break; } } /* Change */ notice = TRUE; } /* Use the value */ p_ptr->food = v; /* Nothing to notice */ if (!notice) return (FALSE); /* Disturb */ if (disturb_state) disturb(0, 0); /* Recalculate bonuses */ p_ptr->update |= (PU_BONUS); /* Redraw hunger */ p_ptr->redraw |= (PR_STATUS|PR_HP); /* Handle stuff */ handle_stuff(); /* Result */ return (TRUE); }
/** * g2d_blend - blend image data in source and destination buffers. * * @ctx: a pointer to g2d_context structure. * @src: a pointer to g2d_image structure including image and buffer * information to source. * @dst: a pointer to g2d_image structure including image and buffer * information to destination. * @src_x: x start position to source buffer. * @src_y: y start position to source buffer. * @dst_x: x start position to destination buffer. * @dst_y: y start position to destination buffer. * @w: width value to source and destination buffer. * @h: height value to source and destination buffer. * @op: blend operation type. */ int g2d_blend(struct g2d_context *ctx, struct g2d_image *src, struct g2d_image *dst, unsigned int src_x, unsigned int src_y, unsigned int dst_x, unsigned int dst_y, unsigned int w, unsigned int h, enum e_g2d_op op) { union g2d_point_val pt; union g2d_bitblt_cmd_val bitblt; union g2d_blend_func_val blend; unsigned int src_w = 0, src_h = 0, dst_w = 0, dst_h = 0; bitblt.val = 0; blend.val = 0; if (op == G2D_OP_SRC || op == G2D_OP_CLEAR) g2d_add_cmd(ctx, DST_SELECT_REG, G2D_SELECT_MODE_BGCOLOR); else g2d_add_cmd(ctx, DST_SELECT_REG, G2D_SELECT_MODE_NORMAL); g2d_add_cmd(ctx, DST_COLOR_MODE_REG, dst->color_mode); g2d_add_base_addr(ctx, dst, g2d_dst); g2d_add_cmd(ctx, DST_STRIDE_REG, dst->stride); g2d_add_cmd(ctx, SRC_SELECT_REG, src->select_mode); g2d_add_cmd(ctx, SRC_COLOR_MODE_REG, src->color_mode); switch (src->select_mode) { case G2D_SELECT_MODE_NORMAL: g2d_add_base_addr(ctx, src, g2d_src); g2d_add_cmd(ctx, SRC_STRIDE_REG, src->stride); break; case G2D_SELECT_MODE_FGCOLOR: g2d_add_cmd(ctx, FG_COLOR_REG, src->color); break; case G2D_SELECT_MODE_BGCOLOR: g2d_add_cmd(ctx, BG_COLOR_REG, src->color); break; default: fprintf(stderr , "failed to set src.\n"); return -EINVAL; } src_w = w; src_h = h; if (src_x + w > src->width) src_w = src->width - src_x; if (src_y + h > src->height) src_h = src->height - src_y; dst_w = w; dst_h = h; if (dst_x + w > dst->width) dst_w = dst->width - dst_x; if (dst_y + h > dst->height) dst_h = dst->height - dst_y; w = MIN(src_w, dst_w); h = MIN(src_h, dst_h); if (w <= 0 || h <= 0) { fprintf(stderr, "invalid width or height.\n"); g2d_reset(ctx); return -EINVAL; } bitblt.data.alpha_blend_mode = G2D_ALPHA_BLEND_MODE_ENABLE; blend.val = g2d_get_blend_op(op); g2d_add_cmd(ctx, BITBLT_COMMAND_REG, bitblt.val); g2d_add_cmd(ctx, BLEND_FUNCTION_REG, blend.val); pt.val = 0; pt.data.x = src_x; pt.data.y = src_y; g2d_add_cmd(ctx, SRC_LEFT_TOP_REG, pt.val); pt.val = 0; pt.data.x = src_x + w; pt.data.y = src_y + h; g2d_add_cmd(ctx, SRC_RIGHT_BOTTOM_REG, pt.val); pt.val = 0; pt.data.x = dst_x; pt.data.y = dst_y; g2d_add_cmd(ctx, DST_LEFT_TOP_REG, pt.val); pt.val = 0; pt.data.x = dst_x + w; pt.data.y = dst_y + h; g2d_add_cmd(ctx, DST_RIGHT_BOTTOM_REG, pt.val); return g2d_flush(ctx); }
int CPL_STDCALL GDALDitherRGB2PCT( GDALRasterBandH hRed, GDALRasterBandH hGreen, GDALRasterBandH hBlue, GDALRasterBandH hTarget, GDALColorTableH hColorTable, GDALProgressFunc pfnProgress, void * pProgressArg ) { VALIDATE_POINTER1( hRed, "GDALDitherRGB2PCT", CE_Failure ); VALIDATE_POINTER1( hGreen, "GDALDitherRGB2PCT", CE_Failure ); VALIDATE_POINTER1( hBlue, "GDALDitherRGB2PCT", CE_Failure ); VALIDATE_POINTER1( hTarget, "GDALDitherRGB2PCT", CE_Failure ); VALIDATE_POINTER1( hColorTable, "GDALDitherRGB2PCT", CE_Failure ); int nXSize, nYSize; CPLErr err = CE_None; /* -------------------------------------------------------------------- */ /* Validate parameters. */ /* -------------------------------------------------------------------- */ nXSize = GDALGetRasterBandXSize( hRed ); nYSize = GDALGetRasterBandYSize( hRed ); if( GDALGetRasterBandXSize( hGreen ) != nXSize || GDALGetRasterBandYSize( hGreen ) != nYSize || GDALGetRasterBandXSize( hBlue ) != nXSize || GDALGetRasterBandYSize( hBlue ) != nYSize ) { CPLError( CE_Failure, CPLE_IllegalArg, "Green or blue band doesn't match size of red band.\n" ); return CE_Failure; } if( GDALGetRasterBandXSize( hTarget ) != nXSize || GDALGetRasterBandYSize( hTarget ) != nYSize ) { CPLError( CE_Failure, CPLE_IllegalArg, "GDALDitherRGB2PCT(): " "Target band doesn't match size of source bands.\n" ); return CE_Failure; } if( pfnProgress == NULL ) pfnProgress = GDALDummyProgress; /* -------------------------------------------------------------------- */ /* Setup more direct colormap. */ /* -------------------------------------------------------------------- */ int nColors, anPCT[768], iColor; nColors = GDALGetColorEntryCount( hColorTable ); if (nColors == 0 ) { CPLError( CE_Failure, CPLE_IllegalArg, "GDALDitherRGB2PCT(): " "Color table must not be empty.\n" ); return CE_Failure; } else if (nColors > 256) { CPLError( CE_Failure, CPLE_IllegalArg, "GDALDitherRGB2PCT(): " "Color table cannot have more than 256 entries.\n" ); return CE_Failure; } for( iColor = 0; iColor < nColors; iColor++ ) { GDALColorEntry sEntry; GDALGetColorEntryAsRGB( hColorTable, iColor, &sEntry ); anPCT[iColor ] = sEntry.c1; anPCT[iColor+256] = sEntry.c2; anPCT[iColor+512] = sEntry.c3; } /* -------------------------------------------------------------------- */ /* Build a 24bit to 8 bit color mapping. */ /* -------------------------------------------------------------------- */ GByte *pabyColorMap; pabyColorMap = (GByte *) CPLMalloc(C_LEVELS * C_LEVELS * C_LEVELS * sizeof(int)); FindNearestColor( nColors, anPCT, pabyColorMap ); /* -------------------------------------------------------------------- */ /* Setup various variables. */ /* -------------------------------------------------------------------- */ GByte *pabyRed, *pabyGreen, *pabyBlue, *pabyIndex; int *panError; pabyRed = (GByte *) VSIMalloc(nXSize); pabyGreen = (GByte *) VSIMalloc(nXSize); pabyBlue = (GByte *) VSIMalloc(nXSize); pabyIndex = (GByte *) VSIMalloc(nXSize); panError = (int *) VSICalloc(sizeof(int),(nXSize+2) * 3); if (pabyRed == NULL || pabyGreen == NULL || pabyBlue == NULL || pabyIndex == NULL || panError == NULL) { CPLError( CE_Failure, CPLE_OutOfMemory, "VSIMalloc(): Out of memory in GDALDitherRGB2PCT" ); err = CE_Failure; goto end_and_cleanup; } /* ==================================================================== */ /* Loop over all scanlines of data to process. */ /* ==================================================================== */ int iScanline; for( iScanline = 0; iScanline < nYSize; iScanline++ ) { int nLastRedError, nLastGreenError, nLastBlueError, i; /* -------------------------------------------------------------------- */ /* Report progress */ /* -------------------------------------------------------------------- */ if( !pfnProgress( iScanline / (double) nYSize, NULL, pProgressArg ) ) { CPLError( CE_Failure, CPLE_UserInterrupt, "User Terminated" ); err = CE_Failure; goto end_and_cleanup; } /* -------------------------------------------------------------------- */ /* Read source data. */ /* -------------------------------------------------------------------- */ GDALRasterIO( hRed, GF_Read, 0, iScanline, nXSize, 1, pabyRed, nXSize, 1, GDT_Byte, 0, 0 ); GDALRasterIO( hGreen, GF_Read, 0, iScanline, nXSize, 1, pabyGreen, nXSize, 1, GDT_Byte, 0, 0 ); GDALRasterIO( hBlue, GF_Read, 0, iScanline, nXSize, 1, pabyBlue, nXSize, 1, GDT_Byte, 0, 0 ); /* -------------------------------------------------------------------- */ /* Apply the error from the previous line to this one. */ /* -------------------------------------------------------------------- */ for( i = 0; i < nXSize; i++ ) { pabyRed[i] = (GByte) MAX(0,MIN(255,(pabyRed[i] + panError[i*3+0+3]))); pabyGreen[i] = (GByte) MAX(0,MIN(255,(pabyGreen[i] + panError[i*3+1+3]))); pabyBlue[i] = (GByte) MAX(0,MIN(255,(pabyBlue[i] + panError[i*3+2+3]))); } memset( panError, 0, sizeof(int) * (nXSize+2) * 3 ); /* -------------------------------------------------------------------- */ /* Figure out the nearest color to the RGB value. */ /* -------------------------------------------------------------------- */ nLastRedError = 0; nLastGreenError = 0; nLastBlueError = 0; for( i = 0; i < nXSize; i++ ) { int iIndex, nError, nSixth, iRed, iGreen, iBlue; int nRedValue, nGreenValue, nBlueValue; nRedValue = MAX(0,MIN(255, pabyRed[i] + nLastRedError)); nGreenValue = MAX(0,MIN(255, pabyGreen[i] + nLastGreenError)); nBlueValue = MAX(0,MIN(255, pabyBlue[i] + nLastBlueError)); iRed = nRedValue * C_LEVELS / 256; iGreen = nGreenValue * C_LEVELS / 256; iBlue = nBlueValue * C_LEVELS / 256; iIndex = pabyColorMap[iRed + iGreen * C_LEVELS + iBlue * C_LEVELS * C_LEVELS]; pabyIndex[i] = (GByte) iIndex; /* -------------------------------------------------------------------- */ /* Compute Red error, and carry it on to the next error line. */ /* -------------------------------------------------------------------- */ nError = nRedValue - anPCT[iIndex ]; nSixth = nError / 6; panError[i*3 ] += nSixth; panError[i*3+6 ] = nSixth; panError[i*3+3 ] += nError - 5 * nSixth; nLastRedError = 2 * nSixth; /* -------------------------------------------------------------------- */ /* Compute Green error, and carry it on to the next error line. */ /* -------------------------------------------------------------------- */ nError = nGreenValue - anPCT[iIndex+256]; nSixth = nError / 6; panError[i*3 +1] += nSixth; panError[i*3+6+1] = nSixth; panError[i*3+3+1] += nError - 5 * nSixth; nLastGreenError = 2 * nSixth; /* -------------------------------------------------------------------- */ /* Compute Blue error, and carry it on to the next error line. */ /* -------------------------------------------------------------------- */ nError = nBlueValue - anPCT[iIndex+512]; nSixth = nError / 6; panError[i*3 +2] += nSixth; panError[i*3+6+2] = nSixth; panError[i*3+3+2] += nError - 5 * nSixth; nLastBlueError = 2 * nSixth; } /* -------------------------------------------------------------------- */ /* Write results. */ /* -------------------------------------------------------------------- */ GDALRasterIO( hTarget, GF_Write, 0, iScanline, nXSize, 1, pabyIndex, nXSize, 1, GDT_Byte, 0, 0 ); } pfnProgress( 1.0, NULL, pProgressArg ); /* -------------------------------------------------------------------- */ /* Cleanup */ /* -------------------------------------------------------------------- */ end_and_cleanup: CPLFree( pabyRed ); CPLFree( pabyGreen ); CPLFree( pabyBlue ); CPLFree( pabyIndex ); CPLFree( panError ); CPLFree( pabyColorMap ); return err; }
/// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance /// updated velocity sent directly to position controller void AC_Loiter::calc_desired_velocity(float nav_dt) { float ekfGndSpdLimit, ekfNavVelGainScaler; AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); // calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED // parameter and the value set by the EKF to observe optical flow limits float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f); gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN); float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f)); // range check nav_dt if (nav_dt < 0) { return; } _pos_control.set_max_speed_xy(gnd_speed_limit_cms); _pos_control.set_max_accel_xy(_accel_cmss); _pos_control.set_leash_length_xy(LOITER_POS_CORRECTION_MAX); // get loiters desired velocity from the position controller where it is being stored. const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity(); Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y); // update the desired velocity using our predicted acceleration desired_vel.x += _predicted_accel.x * nav_dt; desired_vel.y += _predicted_accel.y * nav_dt; Vector2f loiter_accel_brake; float desired_speed = desired_vel.length(); if (!is_zero(desired_speed)) { Vector2f desired_vel_norm = desired_vel/desired_speed; // TODO: consider using a velocity squared relationship like // pilot_acceleration_max*(desired_speed/gnd_speed_limit_cms)^2; // the drag characteristic of a multirotor should be examined to generate a curve // we could add a expo function here to fine tune it // calculate a drag acceleration based on the desired speed. float drag_decel = pilot_acceleration_max*desired_speed/gnd_speed_limit_cms; // calculate a braking acceleration if sticks are at zero float loiter_brake_accel = 0.0f; if (_desired_accel.is_zero()) { if ((AP_HAL::millis()-_brake_timer) > _brake_delay * 1000.0f) { float brake_gain = _pos_control.get_vel_xy_pid().kP() * 0.5f; loiter_brake_accel = constrain_float(AC_AttitudeControl::sqrt_controller(desired_speed, brake_gain, _brake_jerk_max_cmsss, nav_dt), 0.0f, _brake_accel_cmss); } } else { loiter_brake_accel = 0.0f; _brake_timer = AP_HAL::millis(); } _brake_accel += constrain_float(loiter_brake_accel-_brake_accel, -_brake_jerk_max_cmsss*nav_dt, _brake_jerk_max_cmsss*nav_dt); loiter_accel_brake = desired_vel_norm*_brake_accel; // update the desired velocity using the drag and braking accelerations desired_speed = MAX(desired_speed-(drag_decel+_brake_accel)*nav_dt,0.0f); desired_vel = desired_vel_norm*desired_speed; } // add braking to the desired acceleration _desired_accel -= loiter_accel_brake; // Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing) float horizSpdDem = desired_vel.length(); if (horizSpdDem > gnd_speed_limit_cms) { desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem; desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem; } // Limit the velocity to prevent fence violations // TODO: We need to also limit the _desired_accel if (_avoid != nullptr) { _avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _accel_cmss, desired_vel, nav_dt); } // send adjusted feed forward acceleration and velocity back to the Position Controller _pos_control.set_desired_accel_xy(_desired_accel.x, _desired_accel.y); _pos_control.set_desired_velocity_xy(desired_vel.x, desired_vel.y); }
int solver(int m,int n,int nz,int *iA, int *kA, double *A, double *b, double *c, double f, double *x, double *y, double *w, double *z) { double *dx, *dw, *dy, *dz; /* step directions */ double *fx, *fy, *gx, *gy; double phi, psi, dphi, dpsi; double *rho, *sigma, normr, norms; /* infeasibilites */ double *D, *E; /* diagonal matrices */ double gamma, beta, delta, mu, theta; /* parameters */ double *At; /* arrays for A^T */ int *iAt, *kAt; int i,j,iter,v=1,status=5; double primal_obj, dual_obj; /******************************************************************* * Allocate memory for arrays. *******************************************************************/ MALLOC( dx, n, double ); MALLOC( dw, m, double ); MALLOC( dy, m, double ); MALLOC( dz, n, double ); MALLOC( rho, m, double ); MALLOC( sigma, n, double ); MALLOC( D, n, double ); MALLOC( E, m, double ); MALLOC( fx, n, double ); MALLOC( fy, m, double ); MALLOC( gx, n, double ); MALLOC( gy, m, double ); MALLOC( At, nz, double ); MALLOC( iAt, nz, int ); MALLOC( kAt, m+1, int ); /**************************************************************** * Initialization. * ****************************************************************/ for (j=0; j<n; j++) { x[j] = 1.0; z[j] = 1.0; } for (i=0; i<m; i++) { w[i] = 1.0; y[i] = 1.0; } phi = 1.0; psi = 1.0; atnum(m,n,kA,iA,A,kAt,iAt,At); /**************************************************************** * Display Banner. ****************************************************************/ printf ("m = %d,n = %d,nz = %d\n",m,n,nz); printf( "--------------------------------------------------------------------------\n" " | Primal | Dual | |\n" " Iter | Obj Value Infeas | Obj Value Infeas | mu |\n" "- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \n" ); fflush(stdout); /**************************************************************** * Iteration. ****************************************************************/ beta = 0.80; delta = 2*(1-beta); for (iter=0; iter<MAX_ITER; iter++) { /************************************************************* * STEP 1: Compute mu. *************************************************************/ mu = (dotprod(z,x,n)+dotprod(w,y,m)+phi*psi) / (n+m+1); /************************************************************* * STEP 1: Compute primal and dual objective function values. *************************************************************/ primal_obj = dotprod(c,x,n); dual_obj = dotprod(b,y,m); /************************************************************* * STEP 2: Check stopping rule. *************************************************************/ if ( mu < EPS ) { if ( phi > EPS ) { status = 0; break; /* OPTIMAL */ } else if ( dual_obj < 0.0) { status = 2; break; /* PRIMAL INFEASIBLE */ } else if ( primal_obj > 0.0) { status = 4; break; /* DUAL INFEASIBLE */ } else { status = 7; /* NUMERICAL PROBLEM */ break; } } /************************************************************* * STEP 3: Compute infeasibilities. *************************************************************/ smx(m,n,A,kA,iA,x,rho); for (i=0; i<m; i++) { rho[i] = rho[i] - b[i]*phi + w[i]; } normr = sqrt( dotprod(rho,rho,m) )/phi; for (i=0; i<m; i++) { rho[i] = -(1-delta)*rho[i] + w[i] - delta*mu/y[i]; } smx(n,m,At,kAt,iAt,y,sigma); for (j=0; j<n; j++) { sigma[j] = -sigma[j] + c[j]*phi + z[j]; } norms = sqrt( dotprod(sigma,sigma,n) )/phi; for (j=0; j<n; j++) { sigma[j] = -(1-delta)*sigma[j] + z[j] - delta*mu/x[j]; } gamma = -(1-delta)*(dual_obj - primal_obj + psi) + psi - delta*mu/phi; /************************************************************* * Print statistics. *************************************************************/ printf("%8d %14.7e %8.1e %14.7e %8.1e %8.1e \n", iter, high(primal_obj/phi+f), high(normr), high(dual_obj/phi+f), high(norms), high(mu) ); fflush(stdout); /************************************************************* * STEP 4: Compute step directions. *************************************************************/ for (j=0; j<n; j++) { D[j] = z[j]/x[j]; } for (i=0; i<m; i++) { E[i] = w[i]/y[i]; } ldltfac(n, m, kAt, iAt, At, E, D, kA, iA, A, v); for (j=0; j<n; j++) { fx[j] = -sigma[j]; } for (i=0; i<m; i++) { fy[i] = rho[i]; } forwardbackward(E, D, fy, fx); for (j=0; j<n; j++) { gx[j] = -c[j]; } for (i=0; i<m; i++) { gy[i] = -b[i]; } forwardbackward(E, D, gy, gx); dphi = (dotprod(c,fx,n)-dotprod(b,fy,m)+gamma)/ (dotprod(c,gx,n)-dotprod(b,gy,m)-psi/phi); for (j=0; j<n; j++) { dx[j] = fx[j] - gx[j]*dphi; } for (i=0; i<m; i++) { dy[i] = fy[i] - gy[i]*dphi; } for (j=0; j<n; j++) { dz[j] = delta*mu/x[j] - z[j] - D[j]*dx[j]; } for (i=0; i<m; i++) { dw[i] = delta*mu/y[i] - w[i] - E[i]*dy[i]; } dpsi = delta*mu/phi - psi - (psi/phi)*dphi; /************************************************************* * STEP 5: Compute step length. *************************************************************/ theta = 1.0; for (j=0; j<n; j++) { theta = MIN(theta, linesearch(x[j],z[j],dx[j],dz[j],beta,delta,mu)); } for (i=0; i<m; i++) { theta = MIN(theta,linesearch(y[i],w[i],dy[i],dw[i],beta,delta,mu)); } theta = MIN(theta,linesearch(phi,psi,dphi,dpsi,beta,delta,mu)); /* if (theta < 4*beta/(n+m+1)) { printf("ratio = %10.3e \n", theta*(n+m+1)/(4*beta)); status = 7; break; } */ if (theta < 1.0) theta *= 0.9999; /************************************************************* * STEP 6: Step to new point *************************************************************/ for (j=0; j<n; j++) { x[j] = x[j] + theta*dx[j]; z[j] = z[j] + theta*dz[j]; } for (i=0; i<m; i++) { y[i] = y[i] + theta*dy[i]; w[i] = w[i] + theta*dw[i]; } phi = phi + theta*dphi; psi = psi + theta*dpsi; } for (j=0; j<n; j++) { x[j] /= phi; z[j] /= phi; } for (i=0; i<m; i++) { y[i] /= phi; w[i] /= phi; } /**************************************************************** * Free work space * ****************************************************************/ FREE( w ); FREE( z ); FREE( dx ); FREE( dw ); FREE( dy ); FREE( dz ); FREE( rho ); FREE( sigma ); FREE( D ); FREE( E ); FREE( fx ); FREE( fy ); FREE( gx ); FREE( gy ); FREE( At ); FREE( iAt ); FREE( kAt ); return status; } /* End of solver */
int main (int argc, char **argv) { FT_Error error; FT_Library library; FT_Face face; GFile *file; gint font_size, thumb_size = THUMB_SIZE; gchar *thumbstr_utf8 = NULL, *help, *uri; gchar **arguments = NULL; GOptionContext *context; GError *gerror = NULL; gchar *contents = NULL; gboolean retval, default_thumbstr = TRUE; gint rv = 1; GdkRGBA white = { 1.0, 1.0, 1.0, 1.0 }; GdkRGBA black = { 0.0, 0.0, 0.0, 1.0 }; cairo_surface_t *surface; cairo_t *cr; cairo_text_extents_t text_extents; cairo_font_face_t *font; gchar *str; gdouble scale, scale_x, scale_y; const GOptionEntry options[] = { { "text", 't', 0, G_OPTION_ARG_STRING, &thumbstr_utf8, N_("Text to thumbnail (default: Aa)"), N_("TEXT") }, { "size", 's', 0, G_OPTION_ARG_INT, &thumb_size, N_("Thumbnail size (default: 128)"), N_("SIZE") }, { G_OPTION_REMAINING, 0, 0, G_OPTION_ARG_FILENAME_ARRAY, &arguments, NULL, N_("FONT-FILE OUTPUT-FILE") }, { NULL } }; bindtextdomain (GETTEXT_PACKAGE, GNOMELOCALEDIR); bind_textdomain_codeset (GETTEXT_PACKAGE, "UTF-8"); textdomain (GETTEXT_PACKAGE); setlocale (LC_ALL, ""); g_type_init (); context = g_option_context_new (NULL); g_option_context_add_main_entries (context, options, GETTEXT_PACKAGE); retval = g_option_context_parse (context, &argc, &argv, &gerror); if (!retval) { g_printerr ("Error parsing arguments: %s\n", gerror->message); g_option_context_free (context); g_error_free (gerror); return 1; } if (!arguments || g_strv_length (arguments) != 2) { help = g_option_context_get_help (context, TRUE, NULL); g_printerr ("%s", help); g_option_context_free (context); goto out; } g_option_context_free (context); if (thumbstr_utf8 != NULL) default_thumbstr = FALSE; error = FT_Init_FreeType (&library); if (error) { g_printerr("Could not initialise freetype: %s\n", get_ft_error (error)); goto out; } totem_resources_monitor_start (arguments[0], 30 * G_USEC_PER_SEC); file = g_file_new_for_commandline_arg (arguments[0]); uri = g_file_get_uri (file); g_object_unref (file); face = sushi_new_ft_face_from_uri (library, uri, &contents, &gerror); if (gerror) { g_printerr ("Could not load face '%s': %s\n", uri, gerror->message); g_free (uri); g_error_free (gerror); goto out; } g_free (uri); if (default_thumbstr) { if (check_font_contain_text (face, "Aa")) str = g_strdup ("Aa"); else str = build_fallback_thumbstr (face); } else { str = thumbstr_utf8; } surface = cairo_image_surface_create (CAIRO_FORMAT_ARGB32, thumb_size, thumb_size); cr = cairo_create (surface); gdk_cairo_set_source_rgba (cr, &white); cairo_paint (cr); font = cairo_ft_font_face_create_for_ft_face (face, 0); cairo_set_font_face (cr, font); cairo_font_face_destroy (font); font_size = thumb_size - 2 * PADDING_VERTICAL; cairo_set_font_size (cr, font_size); cairo_text_extents (cr, str, &text_extents); if ((text_extents.width) > (thumb_size - 2 * PADDING_HORIZONTAL)) { scale_x = (gdouble) (thumb_size - 2 * PADDING_HORIZONTAL) / (text_extents.width); } else { scale_x = 1.0; } if ((text_extents.height) > (thumb_size - 2 * PADDING_VERTICAL)) { scale_y = (gdouble) (thumb_size - 2 * PADDING_VERTICAL) / (text_extents.height); } else { scale_y = 1.0; } scale = MIN (scale_x, scale_y); cairo_scale (cr, scale, scale); cairo_translate (cr, PADDING_HORIZONTAL - text_extents.x_bearing + (thumb_size - scale * text_extents.width) / 2.0, PADDING_VERTICAL - text_extents.y_bearing + (thumb_size - scale * text_extents.height) / 2.0); gdk_cairo_set_source_rgba (cr, &black); cairo_show_text (cr, str); cairo_destroy (cr); cairo_surface_write_to_png (surface, arguments[1]); cairo_surface_destroy (surface); totem_resources_monitor_stop (); error = FT_Done_Face (face); if (error) { g_printerr("Could not unload face: %s\n", get_ft_error (error)); goto out; } error = FT_Done_FreeType (library); if (error) { g_printerr ("Could not finalize freetype library: %s\n", get_ft_error (error)); goto out; } rv = 0; /* success */ out: g_strfreev (arguments); g_free (str); g_free (contents); return rv; }
/* * Returns true iff the specified sector is present in the disk image. Drivers * not implementing the functionality are assumed to not support backing files, * hence all their sectors are reported as allocated. * * If 'sector_num' is beyond the end of the disk image the return value is 0 * and 'pnum' is set to 0. * * 'pnum' is set to the number of sectors (including and immediately following * the specified sector) that are known to be in the same * allocated/unallocated state. * * 'nb_sectors' is the max value 'pnum' should be set to. If nb_sectors goes * beyond the end of the disk image it will be clamped. */ static int coroutine_fn raw_co_is_allocated(BlockDriverState *bs, int64_t sector_num, int nb_sectors, int *pnum) { off_t start, data, hole; int ret; ret = fd_open(bs); if (ret < 0) { return ret; } start = sector_num * BDRV_SECTOR_SIZE; #ifdef CONFIG_FIEMAP BDRVRawState *s = bs->opaque; struct { struct fiemap fm; struct fiemap_extent fe; } f; f.fm.fm_start = start; f.fm.fm_length = (int64_t)nb_sectors * BDRV_SECTOR_SIZE; f.fm.fm_flags = 0; f.fm.fm_extent_count = 1; f.fm.fm_reserved = 0; if (ioctl(s->fd, FS_IOC_FIEMAP, &f) == -1) { /* Assume everything is allocated. */ *pnum = nb_sectors; return 1; } if (f.fm.fm_mapped_extents == 0) { /* No extents found, data is beyond f.fm.fm_start + f.fm.fm_length. * f.fm.fm_start + f.fm.fm_length must be clamped to the file size! */ off_t length = lseek(s->fd, 0, SEEK_END); hole = f.fm.fm_start; data = MIN(f.fm.fm_start + f.fm.fm_length, length); } else { data = f.fe.fe_logical; hole = f.fe.fe_logical + f.fe.fe_length; } #elif defined SEEK_HOLE && defined SEEK_DATA BDRVRawState *s = bs->opaque; hole = lseek(s->fd, start, SEEK_HOLE); if (hole == -1) { /* -ENXIO indicates that sector_num was past the end of the file. * There is a virtual hole there. */ assert(errno != -ENXIO); /* Most likely EINVAL. Assume everything is allocated. */ *pnum = nb_sectors; return 1; } if (hole > start) { data = start; } else { /* On a hole. We need another syscall to find its end. */ data = lseek(s->fd, start, SEEK_DATA); if (data == -1) { data = lseek(s->fd, 0, SEEK_END); } } #else *pnum = nb_sectors; return 1; #endif if (data <= start) { /* On a data extent, compute sectors to the end of the extent. */ *pnum = MIN(nb_sectors, (hole - start) / BDRV_SECTOR_SIZE); return 1; } else { /* On a hole, compute sectors to the beginning of the next extent. */ *pnum = MIN(nb_sectors, (data - start) / BDRV_SECTOR_SIZE); return 0; } }
size_t ZSTDMT_compressCCtx(ZSTDMT_CCtx* mtctx, void* dst, size_t dstCapacity, const void* src, size_t srcSize, int compressionLevel) { ZSTD_parameters params = ZSTD_getParams(compressionLevel, srcSize, 0); U32 const overlapLog = (compressionLevel >= ZSTD_maxCLevel()) ? 0 : 3; size_t const overlapSize = (size_t)1 << (params.cParams.windowLog - overlapLog); size_t const chunkTargetSize = (size_t)1 << (params.cParams.windowLog + 2); unsigned const nbChunksMax = (unsigned)(srcSize / chunkTargetSize) + 1; unsigned nbChunks = MIN(nbChunksMax, mtctx->nbThreads); size_t const proposedChunkSize = (srcSize + (nbChunks-1)) / nbChunks; size_t const avgChunkSize = ((proposedChunkSize & 0x1FFFF) < 0xFFFF) ? proposedChunkSize + 0xFFFF : proposedChunkSize; /* avoid too small last block */ size_t remainingSrcSize = srcSize; const char* const srcStart = (const char*)src; unsigned const compressWithinDst = (dstCapacity >= ZSTD_compressBound(srcSize)) ? nbChunks : (unsigned)(dstCapacity / ZSTD_compressBound(avgChunkSize)); /* presumes avgChunkSize >= 256 KB, which should be the case */ size_t frameStartPos = 0, dstBufferPos = 0; DEBUGLOG(3, "windowLog : %2u => chunkTargetSize : %u bytes ", params.cParams.windowLog, (U32)chunkTargetSize); DEBUGLOG(2, "nbChunks : %2u (chunkSize : %u bytes) ", nbChunks, (U32)avgChunkSize); params.fParams.contentSizeFlag = 1; if (nbChunks==1) { /* fallback to single-thread mode */ ZSTD_CCtx* const cctx = mtctx->cctxPool->cctx[0]; return ZSTD_compressCCtx(cctx, dst, dstCapacity, src, srcSize, compressionLevel); } { unsigned u; for (u=0; u<nbChunks; u++) { size_t const chunkSize = MIN(remainingSrcSize, avgChunkSize); size_t const dstBufferCapacity = ZSTD_compressBound(chunkSize); buffer_t const dstAsBuffer = { (char*)dst + dstBufferPos, dstBufferCapacity }; buffer_t const dstBuffer = u < compressWithinDst ? dstAsBuffer : ZSTDMT_getBuffer(mtctx->buffPool, dstBufferCapacity); ZSTD_CCtx* const cctx = ZSTDMT_getCCtx(mtctx->cctxPool); size_t dictSize = u ? overlapSize : 0; if ((cctx==NULL) || (dstBuffer.start==NULL)) { mtctx->jobs[u].cSize = ERROR(memory_allocation); /* job result */ mtctx->jobs[u].jobCompleted = 1; nbChunks = u+1; break; /* let's wait for previous jobs to complete, but don't start new ones */ } mtctx->jobs[u].srcStart = srcStart + frameStartPos - dictSize; mtctx->jobs[u].dictSize = dictSize; mtctx->jobs[u].srcSize = chunkSize; mtctx->jobs[u].fullFrameSize = srcSize; mtctx->jobs[u].params = params; mtctx->jobs[u].dstBuff = dstBuffer; mtctx->jobs[u].cctx = cctx; mtctx->jobs[u].firstChunk = (u==0); mtctx->jobs[u].lastChunk = (u==nbChunks-1); mtctx->jobs[u].jobCompleted = 0; mtctx->jobs[u].jobCompleted_mutex = &mtctx->jobCompleted_mutex; mtctx->jobs[u].jobCompleted_cond = &mtctx->jobCompleted_cond; DEBUGLOG(3, "posting job %u (%u bytes)", u, (U32)chunkSize); DEBUG_PRINTHEX(3, mtctx->jobs[u].srcStart, 12); POOL_add(mtctx->factory, ZSTDMT_compressChunk, &mtctx->jobs[u]); frameStartPos += chunkSize; dstBufferPos += dstBufferCapacity; remainingSrcSize -= chunkSize; } } /* note : since nbChunks <= nbThreads, all jobs should be running immediately in parallel */ { unsigned chunkID; size_t error = 0, dstPos = 0; for (chunkID=0; chunkID<nbChunks; chunkID++) { DEBUGLOG(3, "waiting for chunk %u ", chunkID); PTHREAD_MUTEX_LOCK(&mtctx->jobCompleted_mutex); while (mtctx->jobs[chunkID].jobCompleted==0) { DEBUGLOG(4, "waiting for jobCompleted signal from chunk %u", chunkID); pthread_cond_wait(&mtctx->jobCompleted_cond, &mtctx->jobCompleted_mutex); } pthread_mutex_unlock(&mtctx->jobCompleted_mutex); DEBUGLOG(3, "ready to write chunk %u ", chunkID); ZSTDMT_releaseCCtx(mtctx->cctxPool, mtctx->jobs[chunkID].cctx); mtctx->jobs[chunkID].cctx = NULL; mtctx->jobs[chunkID].srcStart = NULL; { size_t const cSize = mtctx->jobs[chunkID].cSize; if (ZSTD_isError(cSize)) error = cSize; if ((!error) && (dstPos + cSize > dstCapacity)) error = ERROR(dstSize_tooSmall); if (chunkID) { /* note : chunk 0 is already written directly into dst */ if (!error) memmove((char*)dst + dstPos, mtctx->jobs[chunkID].dstBuff.start, cSize); /* may overlap if chunk decompressed within dst */ if (chunkID >= compressWithinDst) /* otherwise, it decompresses within dst */ ZSTDMT_releaseBuffer(mtctx->buffPool, mtctx->jobs[chunkID].dstBuff); mtctx->jobs[chunkID].dstBuff = g_nullBuffer; } dstPos += cSize ; } } if (!error) DEBUGLOG(3, "compressed size : %u ", (U32)dstPos); return error ? error : dstPos; } }
static size_t ZSTDMT_createCompressionJob(ZSTDMT_CCtx* zcs, size_t srcSize, unsigned endFrame) { size_t const dstBufferCapacity = ZSTD_compressBound(srcSize); buffer_t const dstBuffer = ZSTDMT_getBuffer(zcs->buffPool, dstBufferCapacity); ZSTD_CCtx* const cctx = ZSTDMT_getCCtx(zcs->cctxPool); unsigned const jobID = zcs->nextJobID & zcs->jobIDMask; if ((cctx==NULL) || (dstBuffer.start==NULL)) { zcs->jobs[jobID].jobCompleted = 1; zcs->nextJobID++; ZSTDMT_waitForAllJobsCompleted(zcs); ZSTDMT_releaseAllJobResources(zcs); return ERROR(memory_allocation); } DEBUGLOG(4, "preparing job %u to compress %u bytes with %u preload ", zcs->nextJobID, (U32)srcSize, (U32)zcs->dictSize); zcs->jobs[jobID].src = zcs->inBuff.buffer; zcs->jobs[jobID].srcStart = zcs->inBuff.buffer.start; zcs->jobs[jobID].srcSize = srcSize; zcs->jobs[jobID].dictSize = zcs->dictSize; /* note : zcs->inBuff.filled is presumed >= srcSize + dictSize */ zcs->jobs[jobID].params = zcs->params; if (zcs->nextJobID) zcs->jobs[jobID].params.fParams.checksumFlag = 0; /* do not calculate checksum within sections, just keep it in header for first section */ zcs->jobs[jobID].cdict = zcs->nextJobID==0 ? zcs->cdict : NULL; zcs->jobs[jobID].fullFrameSize = zcs->frameContentSize; zcs->jobs[jobID].dstBuff = dstBuffer; zcs->jobs[jobID].cctx = cctx; zcs->jobs[jobID].firstChunk = (zcs->nextJobID==0); zcs->jobs[jobID].lastChunk = endFrame; zcs->jobs[jobID].jobCompleted = 0; zcs->jobs[jobID].dstFlushed = 0; zcs->jobs[jobID].jobCompleted_mutex = &zcs->jobCompleted_mutex; zcs->jobs[jobID].jobCompleted_cond = &zcs->jobCompleted_cond; /* get a new buffer for next input */ if (!endFrame) { size_t const newDictSize = MIN(srcSize + zcs->dictSize, zcs->targetDictSize); zcs->inBuff.buffer = ZSTDMT_getBuffer(zcs->buffPool, zcs->inBuffSize); if (zcs->inBuff.buffer.start == NULL) { /* not enough memory to allocate next input buffer */ zcs->jobs[jobID].jobCompleted = 1; zcs->nextJobID++; ZSTDMT_waitForAllJobsCompleted(zcs); ZSTDMT_releaseAllJobResources(zcs); return ERROR(memory_allocation); } DEBUGLOG(5, "inBuff filled to %u", (U32)zcs->inBuff.filled); zcs->inBuff.filled -= srcSize + zcs->dictSize - newDictSize; DEBUGLOG(5, "new job : filled to %u, with %u dict and %u src", (U32)zcs->inBuff.filled, (U32)newDictSize, (U32)(zcs->inBuff.filled - newDictSize)); memmove(zcs->inBuff.buffer.start, (const char*)zcs->jobs[jobID].srcStart + zcs->dictSize + srcSize - newDictSize, zcs->inBuff.filled); DEBUGLOG(5, "new inBuff pre-filled"); zcs->dictSize = newDictSize; } else { zcs->inBuff.buffer = g_nullBuffer; zcs->inBuff.filled = 0; zcs->dictSize = 0; zcs->frameEnded = 1; if (zcs->nextJobID == 0) zcs->params.fParams.checksumFlag = 0; /* single chunk : checksum is calculated directly within worker thread */ } DEBUGLOG(3, "posting job %u : %u bytes (end:%u) (note : doneJob = %u=>%u)", zcs->nextJobID, (U32)zcs->jobs[jobID].srcSize, zcs->jobs[jobID].lastChunk, zcs->doneJobID, zcs->doneJobID & zcs->jobIDMask); POOL_add(zcs->factory, ZSTDMT_compressChunk, &zcs->jobs[jobID]); /* this call is blocking when thread worker pool is exhausted */ zcs->nextJobID++; return 0; }
int cfasta_gotoh_pair_wise (Alignment *A,int*ns, int **l_s,Constraint_list *CL) { /*TREATMENT OF THE TERMINAL GAP PENALTIES*/ /*TG_MODE=0---> gop and gep*/ /*TG_MODE=1---> --- gep*/ /*TG_MODE=2---> --- ---*/ int maximise; /*VARIABLES FOR THE MULTIPLE SEQUENCE ALIGNMENT*/ int **tot_diag; int *diag; int ktup; static int n_groups; static char **group_list; int score, new_score; int n_chosen_diag=20; int step; int max_n_chosen_diag; int l1, l2; /********Prepare Penalties******/ maximise=CL->maximise; ktup=CL->ktup; /********************************/ if ( !group_list) { group_list=make_group_aa (&n_groups, CL->matrix_for_aa_group); } l1=strlen (A->seq_al[l_s[0][0]]); l2=strlen (A->seq_al[l_s[1][0]]); if ( !CL->fasta_step) { step=MIN(l1,l2); step=(int) log ((double)MAX(step, 1)); step=MAX(step, 20); } else { step=CL->fasta_step; } tot_diag=evaluate_diagonals ( A, ns, l_s, CL, maximise,n_groups,group_list, ktup); max_n_chosen_diag=strlen (A->seq_al[l_s[0][0]])+strlen (A->seq_al[l_s[1][0]])-1; /*max_n_chosen_diag=(int)log10((double)(l1+l2))*10;*/ n_chosen_diag+=step; n_chosen_diag=MIN(n_chosen_diag, max_n_chosen_diag); diag=extract_N_diag (strlen (A->seq_al[l_s[0][0]]),strlen (A->seq_al[l_s[1][0]]), tot_diag, n_chosen_diag, 0); score =make_fasta_gotoh_pair_wise ( A, ns, l_s, CL, diag); new_score=0; vfree ( diag); while (new_score!=score && n_chosen_diag< max_n_chosen_diag ) { score=new_score; ungap_sub_aln ( A, ns[0], l_s[0]); ungap_sub_aln ( A, ns[1], l_s[1]); n_chosen_diag+=step; n_chosen_diag=MIN(n_chosen_diag, max_n_chosen_diag); diag =extract_N_diag (strlen (A->seq_al[l_s[0][0]]),strlen (A->seq_al[l_s[1][0]]), tot_diag, n_chosen_diag, 0); new_score=make_fasta_gotoh_pair_wise ( A, ns, l_s, CL, diag); vfree ( diag); } score=new_score; free_int (tot_diag, -1); return score; }
/* * The audit system call. Trust what the user has sent down and save it * away in the audit file. User passes a complete audit record and its * length. We will fill in the time stamp, check the header and the length * Put a trailer and a sequence token if policy requires. * In the future length might become size_t instead of an int. * * The call is valid whether or not AUDIT_PERZONE is set (think of * login to a zone). When the local audit state (auk_auditstate) is * AUC_INIT_AUDIT, records are accepted even though auditd isn't * running. */ int audit(caddr_t record, int length) { char c; int count, l; token_t *m, *n, *s, *ad; int hdrlen, delta; adr_t hadr; adr_t sadr; int size; /* 0: 32 bit utility 1: 64 bit utility */ int host_len; size_t zlen; au_kcontext_t *kctx = GET_KCTX_PZ; /* if auditing not enabled, then don't generate an audit record */ if (kctx->auk_auditstate != AUC_AUDITING && kctx->auk_auditstate != AUC_INIT_AUDIT) return (0); /* Only privileged processes can audit */ if (secpolicy_audit_modify(CRED()) != 0) return (EPERM); /* Max user record size is 32K */ if (length > AUDIT_REC_SIZE) return (E2BIG); /* * The specified length must be at least as big as the smallest * possible header token. Later after beginning to scan the * header we'll determine the true minimum length according to * the header type and attributes. */ #define AU_MIN_HEADER_LEN (sizeof (char) + sizeof (int32_t) + \ sizeof (char) + sizeof (short) + sizeof (short) + \ (sizeof (int32_t) * 2)) if (length < AU_MIN_HEADER_LEN) return (EINVAL); /* Read in user's audit record */ count = length; m = n = s = ad = NULL; while (count) { m = au_getclr(); if (!s) s = n = m; else { n->next_buf = m; n = m; } l = MIN(count, AU_BUFSIZE); if (copyin(record, memtod(m, caddr_t), (size_t)l)) { /* copyin failed release au_membuf */ au_free_rec(s); return (EFAULT); } record += l; count -= l; m->len = (uchar_t)l; } /* Now attach the entire thing to ad */ au_write((caddr_t *)&(ad), s); /* validate header token type. trust everything following it */ adr_start(&hadr, memtod(s, char *)); (void) adr_getchar(&hadr, &c); switch (c) { case AUT_HEADER32: /* size vers+event_ID+event_modifier fields */ delta = 1 + 2 + 2; hdrlen = 1 + 4 + delta + (sizeof (int32_t) * 2); size = HEADER_SIZE32; break; #ifdef _LP64 case AUT_HEADER64: /* size vers+event_ID+event_modifier fields */ delta = 1 + 2 + 2; hdrlen = 1 + 4 + delta + (sizeof (int64_t) * 2); size = HEADER_SIZE64; break; #endif case AUT_HEADER32_EX: /* * Skip over the length/version/type/mod fields and * grab the host address type (length), then rewind. * This is safe per the previous minimum length check. */ hadr.adr_now += 9; (void) adr_getint32(&hadr, &host_len); hadr.adr_now -= 9 + sizeof (int32_t); /* size: vers+event_ID+event_modifier+IP_type+IP_addr_array */ delta = 1 + 2 + 2 + 4 + host_len; hdrlen = 1 + 4 + delta + (sizeof (int32_t) * 2); size = HEADER_SIZE32; break; #ifdef _LP64 case AUT_HEADER64_EX: /* * Skip over the length/version/type/mod fields and grab * the host address type (length), then rewind. * This is safe per the previous minimum length check. */ hadr.adr_now += 9; (void) adr_getint32(&hadr, &host_len); hadr.adr_now -= 9 + sizeof (int32_t); /* size: vers+event_ID+event_modifier+IP_type+IP_addr_array */ delta = 1 + 2 + 2 + 4 + host_len; hdrlen = 1 + 4 + delta + (sizeof (int64_t) * 2); size = HEADER_SIZE64; break; #endif default: /* Header is wrong, reject message */ au_free_rec(s); return (EINVAL); } if (length < hdrlen) { au_free_rec(s); return (0); } /* advance over header token length field */ hadr.adr_now += 4; /* validate version */ (void) adr_getchar(&hadr, &c); if (c != TOKEN_VERSION) { /* version is wrong, reject message */ au_free_rec(s); return (EINVAL); } /* backup to header length field (including version field) */ hadr.adr_now -= 5; /* * add on the zonename token if policy AUDIT_ZONENAME is set */ if (kctx->auk_policy & AUDIT_ZONENAME) { zlen = au_zonename_length(NULL); if (zlen > 0) { length += zlen; m = au_to_zonename(zlen, NULL); (void) au_append_rec(ad, m, AU_PACK); } } /* Add an (optional) sequence token. NULL offset if none */ if (kctx->auk_policy & AUDIT_SEQ) { /* get the sequnce token */ m = au_to_seq(); /* sequence token 5 bytes long */ length += 5; /* link to audit record (i.e. don't pack the data) */ (void) au_append_rec(ad, m, AU_LINK); /* advance to count field of token */ adr_start(&sadr, memtod(m, char *)); sadr.adr_now += 1; } else
static int privcmd_ioctl(struct cdev *dev, unsigned long cmd, caddr_t arg, int mode, struct thread *td) { int error, i; switch (cmd) { case IOCTL_PRIVCMD_HYPERCALL: { struct ioctl_privcmd_hypercall *hcall; hcall = (struct ioctl_privcmd_hypercall *)arg; #ifdef __amd64__ /* * The hypervisor page table walker will refuse to access * user-space pages if SMAP is enabled, so temporary disable it * while performing the hypercall. */ if (cpu_stdext_feature & CPUID_STDEXT_SMAP) stac(); #endif error = privcmd_hypercall(hcall->op, hcall->arg[0], hcall->arg[1], hcall->arg[2], hcall->arg[3], hcall->arg[4]); #ifdef __amd64__ if (cpu_stdext_feature & CPUID_STDEXT_SMAP) clac(); #endif if (error >= 0) { hcall->retval = error; error = 0; } else { error = xen_translate_error(error); hcall->retval = 0; } break; } case IOCTL_PRIVCMD_MMAPBATCH: { struct ioctl_privcmd_mmapbatch *mmap; vm_map_t map; vm_map_entry_t entry; vm_object_t mem; vm_pindex_t pindex; vm_prot_t prot; boolean_t wired; struct xen_add_to_physmap_range add; xen_ulong_t *idxs; xen_pfn_t *gpfns; int *errs, index; struct privcmd_map *umap; uint16_t num; mmap = (struct ioctl_privcmd_mmapbatch *)arg; if ((mmap->num == 0) || ((mmap->addr & PAGE_MASK) != 0)) { error = EINVAL; break; } map = &td->td_proc->p_vmspace->vm_map; error = vm_map_lookup(&map, mmap->addr, VM_PROT_NONE, &entry, &mem, &pindex, &prot, &wired); if (error != KERN_SUCCESS) { error = EINVAL; break; } if ((entry->start != mmap->addr) || (entry->end != mmap->addr + (mmap->num * PAGE_SIZE))) { vm_map_lookup_done(map, entry); error = EINVAL; break; } vm_map_lookup_done(map, entry); if ((mem->type != OBJT_MGTDEVICE) || (mem->un_pager.devp.ops != &privcmd_pg_ops)) { error = EINVAL; break; } umap = mem->handle; add.domid = DOMID_SELF; add.space = XENMAPSPACE_gmfn_foreign; add.foreign_domid = mmap->dom; /* * The 'size' field in the xen_add_to_physmap_range only * allows for UINT16_MAX mappings in a single hypercall. */ num = MIN(mmap->num, UINT16_MAX); idxs = malloc(sizeof(*idxs) * num, M_PRIVCMD, M_WAITOK); gpfns = malloc(sizeof(*gpfns) * num, M_PRIVCMD, M_WAITOK); errs = malloc(sizeof(*errs) * num, M_PRIVCMD, M_WAITOK); set_xen_guest_handle(add.idxs, idxs); set_xen_guest_handle(add.gpfns, gpfns); set_xen_guest_handle(add.errs, errs); /* Allocate a bitset to store broken page mappings. */ umap->err = BITSET_ALLOC(mmap->num, M_PRIVCMD, M_WAITOK | M_ZERO); for (index = 0; index < mmap->num; index += num) { num = MIN(mmap->num - index, UINT16_MAX); add.size = num; error = copyin(&mmap->arr[index], idxs, sizeof(idxs[0]) * num); if (error != 0) goto mmap_out; for (i = 0; i < num; i++) gpfns[i] = atop(umap->phys_base_addr + (i + index) * PAGE_SIZE); bzero(errs, sizeof(*errs) * num); error = HYPERVISOR_memory_op( XENMEM_add_to_physmap_range, &add); if (error != 0) { error = xen_translate_error(error); goto mmap_out; } for (i = 0; i < num; i++) { if (errs[i] != 0) { errs[i] = xen_translate_error(errs[i]); /* Mark the page as invalid. */ BIT_SET(mmap->num, index + i, umap->err); } } error = copyout(errs, &mmap->err[index], sizeof(errs[0]) * num); if (error != 0) goto mmap_out; } umap->mapped = true; mmap_out: free(idxs, M_PRIVCMD); free(gpfns, M_PRIVCMD); free(errs, M_PRIVCMD); if (!umap->mapped) free(umap->err, M_PRIVCMD); break; } default: error = ENOSYS; break; } return (error); }
void DrawRegion( Region key, float scale ) { if ( key == NULL ) return; int stable = key->stable; char name[256]; sprintf(name,"/tmp/T%03d.ppm",stable); Image out = ReadPPMFile(name); static int count = 0; if ( !ImageIsGood(out) ) { out = ConvertImage1(CopyImage(key->image)); sprintf(name,"/tmp/R%05d.ppm",count++); } else sprintf(name,"/tmp/T%03d.ppm",stable); fprintf(stderr,"."); int rv = RandomNumber(0,255); int gv = RandomNumber(0,rv); int bv = RandomNumber(0,gv); int color = PIX3(rv,gv,bv); DrawPolygon(key->border,out,color); Ellipse e1 = NewEllipse(key->row,key->col,key->maj*scale,key->min*scale,key->phi); DrawEllipse(e1,out,color); free(e1); Image patch = CreateImage(41*sqrt(2),41*sqrt(2)); RegionToPatch(key,key->image,patch,scale); FVec hist = GenerateOrientationHistogram(patch); GaussianBlur1D(hist->values,hist->l,hist->r,2); DrawFVec(hist,10,10,200,400,PIX3(0,0,250),out); FVecFree(hist); if ( PolygonIsGood(key->sizes) ) { struct PointSt p1 = key->sizes->vertices[0]; struct PointSt p2 = key->sizes->vertices[key->sizes->numberOfVertices-1]; int i; hist = FVecNew(0,255); Point p; while ( ( p = NextPolygonVertex(key->sizes) ) != NULL ) FVecSetAt(hist,p->y,p->x); if ( p1.y < p2.y ) { for(i=p1.y;i<=p2.y;i++) if ( hist->values[i] == 0.0 ) FVecAddAt(hist,i,1); } else { for(i=p2.y;i>=p1.y;i--) if ( hist->values[i] == 0.0 ) FVecAddAt(hist,i,1); } hist->l = MIN(p1.y,p2.y); hist->r = MAX(p2.y-1,p1.y-1); DrawSizeFVec(hist,497,0,1021,1023,color,stable,out); DrawSizeFVec(hist,498,0,1022,1023,color,stable,out); DrawSizeFVec(hist,499,0,1023,1023,color,stable,out); } WritePPM(name,out); FreeImage(out); }
int fs_ml_event_loop(void) { // printf("fs_ml_event_loop\n"); int result = 0; SDL_Event event; while (SDL_PollEvent(&event)) { switch(event.type) { case SDL_QUIT: fs_log("Received SDL_QUIT\n"); fs_ml_quit(); #ifdef FS_EMU_DRIVERS printf("returning 1 from fs_ml_event_loop\n"); result = 1; #endif continue; case SDL_WINDOWEVENT: if (event.window.event == SDL_WINDOWEVENT_RESIZED) { on_resize(event.window.data1, event.window.data2); } else if (event.window.event == SDL_WINDOWEVENT_CLOSE) { event.type = SDL_QUIT; SDL_PushEvent(&event); } else if (event.window.event == SDL_WINDOWEVENT_FOCUS_GAINED) { if (g_grab_input_on_activate) { fs_log("Window focus gained - grabbing input\n"); g_grab_input_on_activate = false; fs_ml_set_input_grab(true); #ifdef MACOSX } else if (fs_ml_input_grab()) { /* Input grab could be "lost" due to Cmd+Tab */ fs_log("Forcing re-grab of input on OS X\n"); fs_ml_set_input_grab(false); fs_ml_set_input_grab(true); #endif } } continue; case SDL_KEYDOWN: case SDL_KEYUP: if (g_fs_log_input) { fs_log("SDL key sym %d mod %d scancode %d state %d repeat %d\n", event.key.keysym.sym, event.key.keysym.mod, event.key.keysym.scancode, event.key.state, event.key.repeat); } if (event.key.repeat) { continue; } if (event.key.keysym.sym == 0 && event.key.keysym.scancode == 0) { /* ignore "ghost key" seen on OS X which without this * specific check will cause the A key to be mysteriously * pressed. */ if (g_fs_log_input) { fs_log("- ignored key with keysym 0 and scancode 0\n"); } continue; } /* if (event.key.keysym.sym == SDLK_F12) { g_f12_state = event.key.state ? FS_ML_KEY_MOD_F12 : 0; printf("-- g_f12_state is %d\n", g_f12_state); } else if (event.key.keysym.sym == SDLK_F11) { g_f11_state = event.key.state ? FS_ML_KEY_MOD_F11 : 0; } */ const Uint8* key_state; int num_keys; key_state = SDL_GetKeyboardState(&num_keys); g_f11_state = key_state[SDL_SCANCODE_F11] ? FS_ML_KEY_MOD_F11 : 0; g_f12_state = key_state[SDL_SCANCODE_F12] ? FS_ML_KEY_MOD_F12 : 0; int key = -1; if (event.key.keysym.scancode <= LAST_SDL2_SCANCODE) { key = g_sdl2_keys[event.key.keysym.scancode]; } #if defined(MACOSX) #elif defined(WINDOWS) #else else if (event.key.keysym.sym == SDLK_MODE) { key = SDLK_RALT; } #endif else { key = fs_ml_scancode_to_key(event.key.keysym.scancode); } #ifdef USE_SDL2 if (0) { // the below trick does not currently work for SDL2, as // there is no mapping yet for translated keys } #else if (g_f12_state || g_f11_state) { // leave translated key code in keysym } #endif else if (key >= 0) { if (g_fs_log_input) { fs_log("- key code set to %d (was %d) based on " "scancode %d\n", key, event.key.keysym.sym, event.key.keysym.scancode); } event.key.keysym.sym = key; } int mod = event.key.keysym.mod; if (mod & KMOD_LSHIFT || mod & KMOD_RSHIFT) event.key.keysym.mod |= KMOD_SHIFT; #if 0 if (mod & KMOD_LALT || mod & KMOD_RALT) event.key.keysym.mod |= KMOD_ALT; #endif if (mod & KMOD_LCTRL || mod & KMOD_RCTRL) event.key.keysym.mod |= KMOD_CTRL; #if 0 if (mod & KMOD_LMETA || mod & KMOD_RMETA) event.key.keysym.mod |= KMOD_META; #endif /* Filter out other modidifers */ event.key.keysym.mod &= KMOD_SHIFT | KMOD_ALT | KMOD_CTRL | KMOD_META; /* Add F11/F12 modifier state */ event.key.keysym.mod |= g_f11_state | g_f12_state; //printf("%d %d %d %d\n", event.key.keysym.mod, // KMOD_ALT, KMOD_LALT, KMOD_RALT); break; //case SDL_MOUSEBUTTONDOWN: // printf("--- mousebutton down ---\n"); } fs_ml_event *new_event = NULL; if (event.type == SDL_KEYDOWN) { new_event = fs_ml_alloc_event(); new_event->type = FS_ML_KEYDOWN; new_event->key.keysym.sym = event.key.keysym.sym; new_event->key.keysym.mod = event.key.keysym.mod; new_event->key.state = event.key.state; } else if (event.type == SDL_KEYUP) { new_event = fs_ml_alloc_event(); new_event->type = FS_ML_KEYUP; new_event->key.keysym.sym = event.key.keysym.sym; new_event->key.keysym.mod = event.key.keysym.mod; new_event->key.state = event.key.state; } else if (event.type == SDL_JOYBUTTONDOWN) { if (g_fs_log_input) { fs_log("SDL_JOYBUTTONDOWN which %d button %d state %d\n", event.jbutton.which, event.jbutton.button, event.jbutton.state); } new_event = fs_ml_alloc_event(); new_event->type = FS_ML_JOYBUTTONDOWN; new_event->jbutton.which = \ g_fs_ml_sdl_joystick_index_map[event.jbutton.which]; new_event->jbutton.button = event.jbutton.button; new_event->jbutton.state = event.jbutton.state; } else if (event.type == SDL_JOYBUTTONUP) { if (g_fs_log_input) { fs_log("SDL_JOYBUTTONUP which %d button %d state %d\n", event.jbutton.which, event.jbutton.button, event.jbutton.state); } new_event = fs_ml_alloc_event(); new_event->type = FS_ML_JOYBUTTONUP; new_event->jbutton.which = \ g_fs_ml_sdl_joystick_index_map[event.jbutton.which]; new_event->jbutton.button = event.jbutton.button; new_event->jbutton.state = event.jbutton.state; } else if (event.type == SDL_JOYAXISMOTION) { /* Not logging axis motion, too much noise */ new_event = fs_ml_alloc_event(); new_event->type = FS_ML_JOYAXISMOTION; new_event->jaxis.which = \ g_fs_ml_sdl_joystick_index_map[event.jaxis.which]; new_event->jaxis.axis = event.jaxis.axis; new_event->jaxis.value = event.jaxis.value; } else if (event.type == SDL_JOYHATMOTION) { if (g_fs_log_input) { fs_log("SDL_JOYHATMOTION which %d hat %d value %d\n", event.jhat.which, event.jhat.hat, event.jhat.value); } new_event = fs_ml_alloc_event(); new_event->type = FS_ML_JOYHATMOTION; new_event->jhat.which = \ g_fs_ml_sdl_joystick_index_map[event.jhat.which]; new_event->jhat.hat = event.jhat.hat; new_event->jhat.value = event.jhat.value; } else if (event.type == SDL_MOUSEMOTION) { new_event = fs_ml_alloc_event(); new_event->type = FS_ML_MOUSEMOTION; new_event->motion.device = g_fs_ml_first_mouse_index; new_event->motion.xrel = event.motion.xrel; new_event->motion.yrel = event.motion.yrel; /* Absolute window coordinates */ new_event->motion.x = event.motion.x; new_event->motion.y = event.motion.y; //printf("ISREL %d\n", SDL_GetRelativeMouseMode()); if (g_fs_log_input) { fs_log("SDL mouse event x: %4d y: %4d xrel: %4d yrel: %4d\n", event.motion.x, event.motion.y, event.motion.xrel, event.motion.yrel); } } else if (event.type == SDL_MOUSEBUTTONDOWN) { new_event = fs_ml_alloc_event(); new_event->type = FS_ML_MOUSEBUTTONDOWN; new_event->button.device = g_fs_ml_first_mouse_index; new_event->button.button = event.button.button; #ifdef MACOSX if (new_event->button.button == 1) { int mod = SDL_GetModState(); if (mod & KMOD_ALT) { new_event->button.button = 2; } else if (mod & KMOD_CTRL) { new_event->button.button = 3; } } #endif new_event->button.state = event.button.state; } else if (event.type == SDL_MOUSEBUTTONUP) { new_event = fs_ml_alloc_event(); new_event->type = FS_ML_MOUSEBUTTONUP; new_event->button.device = g_fs_ml_first_mouse_index; new_event->button.button = event.button.button; #ifdef MACOSX if (new_event->button.button == 1) { int mod = SDL_GetModState(); if (mod & KMOD_ALT) { new_event->button.button = 2; } else if (mod & KMOD_CTRL) { new_event->button.button = 3; } } #endif new_event->button.state = event.button.state; } else if (event.type == SDL_MOUSEWHEEL) { /* if (event.wheel.which == SDL_TOUCH_MOUSEID) { } */ if (event.wheel.y) { if (g_fs_log_input) { fs_log("SDL mouse event y-scroll: %4d\n", event.wheel.y); } new_event = fs_ml_alloc_event(); new_event->type = FS_ML_MOUSEBUTTONDOWN; if (event.wheel.y > 0) { new_event->button.button = FS_ML_BUTTON_WHEELUP; } else { new_event->button.button = FS_ML_BUTTON_WHEELDOWN; } new_event->button.device = g_fs_ml_first_mouse_index; new_event->button.state = 1; } } else if (event.type == SDL_TEXTINPUT) { new_event = fs_ml_alloc_event(); new_event->type = FS_ML_TEXTINPUT; memcpy(&(new_event->text.text), &(event.text.text), MIN(TEXTINPUTEVENT_TEXT_SIZE, SDL_TEXTINPUTEVENT_TEXT_SIZE)); new_event->text.text[TEXTINPUTEVENT_TEXT_SIZE - 1] = 0; } if (new_event) { fs_ml_post_event(new_event); } } return result; }
// sanity check parameters void AC_Loiter::sanity_check_params() { _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN); _accel_cmss = MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f))); }
int decode_audio_file(ChromaprintContext *chromaprint_ctx, int16_t *buffer1, int16_t *buffer2, const char *file_name, int max_length, int *duration) { int i, ok = 0, remaining, length, consumed, buffer_size, codec_ctx_opened = 0; AVFormatContext *format_ctx = NULL; AVCodecContext *codec_ctx = NULL; AVCodec *codec = NULL; AVStream *stream = NULL; AVPacket packet, packet_temp; #ifdef HAVE_AV_AUDIO_CONVERT AVAudioConvert *convert_ctx = NULL; #endif int16_t *buffer; #if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53, 2, 0) if (av_open_input_file(&format_ctx, file_name, NULL, 0, NULL) != 0) { #else if (avformat_open_input(&format_ctx, file_name, NULL, NULL) != 0) { #endif fprintf(stderr, "ERROR: couldn't open the file\n"); goto done; } if (av_find_stream_info(format_ctx) < 0) { fprintf(stderr, "ERROR: couldn't find stream information in the file\n"); goto done; } for (i = 0; i < format_ctx->nb_streams; i++) { codec_ctx = format_ctx->streams[i]->codec; if (codec_ctx && codec_ctx->codec_type == AVMEDIA_TYPE_AUDIO) { stream = format_ctx->streams[i]; break; } } if (!stream) { fprintf(stderr, "ERROR: couldn't find any audio stream in the file\n"); goto done; } codec = avcodec_find_decoder(codec_ctx->codec_id); if (!codec) { fprintf(stderr, "ERROR: unknown codec\n"); goto done; } if (avcodec_open(codec_ctx, codec) < 0) { fprintf(stderr, "ERROR: couldn't open the codec\n"); goto done; } codec_ctx_opened = 1; if (codec_ctx->channels <= 0) { fprintf(stderr, "ERROR: no channels found in the audio stream\n"); goto done; } if (codec_ctx->sample_fmt != AV_SAMPLE_FMT_S16) { #ifdef HAVE_AV_AUDIO_CONVERT convert_ctx = av_audio_convert_alloc(AV_SAMPLE_FMT_S16, codec_ctx->channels, codec_ctx->sample_fmt, codec_ctx->channels, NULL, 0); if (!convert_ctx) { fprintf(stderr, "ERROR: couldn't create sample format converter\n"); goto done; } #else fprintf(stderr, "ERROR: unsupported sample format\n"); goto done; #endif } *duration = stream->time_base.num * stream->duration / stream->time_base.den; if (max_length == 0) { max_length = duration; } av_init_packet(&packet); av_init_packet(&packet_temp); remaining = max_length * codec_ctx->channels * codec_ctx->sample_rate; chromaprint_start(chromaprint_ctx, codec_ctx->sample_rate, codec_ctx->channels); while (1) { if (av_read_frame(format_ctx, &packet) < 0) { break; } packet_temp.data = packet.data; packet_temp.size = packet.size; while (packet_temp.size > 0) { buffer_size = BUFFER_SIZE; #if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52, 23, 0) consumed = avcodec_decode_audio2(codec_ctx, buffer1, &buffer_size, packet_temp.data, packet_temp.size); #else consumed = avcodec_decode_audio3(codec_ctx, buffer1, &buffer_size, &packet_temp); #endif if (consumed < 0) { break; } packet_temp.data += consumed; packet_temp.size -= consumed; if (buffer_size <= 0) { if (buffer_size < 0) { fprintf(stderr, "WARNING: size returned from avcodec_decode_audioX is too small\n"); } continue; } if (buffer_size > BUFFER_SIZE) { fprintf(stderr, "WARNING: size returned from avcodec_decode_audioX is too large\n"); continue; } #ifdef HAVE_AV_AUDIO_CONVERT if (convert_ctx) { const void *ibuf[6] = { buffer1 }; void *obuf[6] = { buffer2 }; #if LIBAVUTIL_VERSION_INT < AV_VERSION_INT(51, 8, 0) int istride[6] = { av_get_bits_per_sample_format(codec_ctx->sample_fmt) / 8 }; #else int istride[6] = { av_get_bytes_per_sample(codec_ctx->sample_fmt) }; #endif int ostride[6] = { 2 }; int len = buffer_size / istride[0]; if (av_audio_convert(convert_ctx, obuf, ostride, ibuf, istride, len) < 0) { break; } buffer = buffer2; buffer_size = len * ostride[0]; } else { buffer = buffer1; } #else buffer = buffer1; #endif length = MIN(remaining, buffer_size / 2); if (!chromaprint_feed(chromaprint_ctx, buffer, length)) { fprintf(stderr, "ERROR: fingerprint calculation failed\n"); goto done; } if (max_length) { remaining -= length; if (remaining <= 0) { goto finish; } } } if (packet.data) { av_free_packet(&packet); } } finish: if (!chromaprint_finish(chromaprint_ctx)) { fprintf(stderr, "ERROR: fingerprint calculation failed\n"); goto done; } ok = 1; done: if (codec_ctx_opened) { avcodec_close(codec_ctx); } if (format_ctx) { av_close_input_file(format_ctx); } #ifdef HAVE_AV_AUDIO_CONVERT if (convert_ctx) { av_audio_convert_free(convert_ctx); } #endif return ok; } int fpcalc_main(int argc, char **argv) { int i, j, max_length = 120, num_file_names = 0, raw = 0, raw_fingerprint_size, duration; int16_t *buffer1, *buffer2; int32_t *raw_fingerprint; char *file_name, *fingerprint, **file_names; ChromaprintContext *chromaprint_ctx; int algo = CHROMAPRINT_ALGORITHM_DEFAULT; file_names = malloc(argc * sizeof(char *)); for (i = 1; i < argc; i++) { char *arg = argv[i]; if (!strcmp(arg, "-length") && i + 1 < argc) { max_length = atoi(argv[++i]); } else if (!strcmp(arg, "-version") || !strcmp(arg, "-v")) { printf("fpcalc version %s\n", chromaprint_get_version()); return 0; } else if (!strcmp(arg, "-raw")) { raw = 1; } else if (!strcmp(arg, "-algo") && i + 1 < argc) { const char *v = argv[++i]; if (!strcmp(v, "test1")) { algo = CHROMAPRINT_ALGORITHM_TEST1; } else if (!strcmp(v, "test2")) { algo = CHROMAPRINT_ALGORITHM_TEST2; } else if (!strcmp(v, "test3")) { algo = CHROMAPRINT_ALGORITHM_TEST3; } else if (!strcmp(v, "test4")) { algo = CHROMAPRINT_ALGORITHM_TEST4; } else { fprintf(stderr, "WARNING: unknown algorithm, using the default\n"); } } else if (!strcmp(arg, "-set") && i + 1 < argc) { char *name = argv[++i]; char *value = strchr(name, '='); if (!value && i + 1 < argc) { ++i; } } else { file_names[num_file_names++] = argv[i]; } } if (!num_file_names) { printf("usage: %s [OPTIONS] FILE...\n\n", argv[0]); printf("Options:\n"); printf(" -version print version information\n"); printf(" -length SECS length of the audio data used for fingerprint calculation (default 120)\n"); printf(" -raw output the raw uncompressed fingerprint\n"); printf(" -algo NAME version of the fingerprint algorithm\n"); return 2; } av_register_all(); av_log_set_level(AV_LOG_ERROR); buffer1 = av_malloc(BUFFER_SIZE + 16); buffer2 = av_malloc(BUFFER_SIZE + 16); chromaprint_ctx = chromaprint_new(algo); for (i = 1; i < argc; i++) { char *arg = argv[i]; if (!strcmp(arg, "-set") && i + 1 < argc) { char *name = argv[++i]; char *value = strchr(name, '='); if (value) { *value++ = '\0'; chromaprint_set_option(chromaprint_ctx, name, atoi(value)); } else if (i + 1 < argc) { value = argv[++i]; chromaprint_set_option(chromaprint_ctx, name, atoi(value)); } } } for (i = 0; i < num_file_names; i++) { file_name = file_names[i]; if (!decode_audio_file(chromaprint_ctx, buffer1, buffer2, file_name, max_length, &duration)) { fprintf(stderr, "ERROR: unable to calculate fingerprint for file %s, skipping\n", file_name); continue; } if (i > 0) { printf("\n"); } printf("FILE=%s\n", file_name); printf("DURATION=%d\n", duration); if (raw) { if (!chromaprint_get_raw_fingerprint(chromaprint_ctx, (void **)&raw_fingerprint, &raw_fingerprint_size)) { fprintf(stderr, "ERROR: unable to calculate fingerprint for file %s, skipping\n", file_name); continue; } printf("FINGERPRINT="); for (j = 0; j < raw_fingerprint_size; j++) { printf("%d%s", raw_fingerprint[j], j + 1 < raw_fingerprint_size ? "," : ""); } printf("\n"); chromaprint_dealloc(raw_fingerprint); } else { if (!chromaprint_get_fingerprint(chromaprint_ctx, &fingerprint)) { fprintf(stderr, "ERROR: unable to calculate fingerprint for file %s, skipping\n", file_name); continue; } printf("FINGERPRINT=%s\n", fingerprint); chromaprint_dealloc(fingerprint); } } chromaprint_free(chromaprint_ctx); av_free(buffer1); av_free(buffer2); free(file_names); return 0; }
/*------------------------------------------------------------------*/ void relation_batch_init(msieve_obj *obj, relation_batch_t *rb, uint32 min_prime, uint32 max_prime, uint32 lp_cutoff_r, uint32 lp_cutoff_a, savefile_t *savefile, print_relation_t print_relation) { prime_sieve_t sieve; uint32 num_primes, p; /* count the number of primes to multiply. Knowing this in advance makes the recursion a lot easier, at the cost of a small penalty in runtime */ init_prime_sieve(&sieve, min_prime + 1, max_prime); p = min_prime; num_primes = 0; while (p < max_prime) { p = get_next_prime(&sieve); num_primes++; } free_prime_sieve(&sieve); /* compute the product of primes */ logprintf(obj, "multiplying %u primes from %u to %u\n", num_primes, min_prime, max_prime); init_prime_sieve(&sieve, min_prime, max_prime); mpz_init(rb->prime_product); multiply_primes(0, num_primes - 2, &sieve, rb->prime_product); logprintf(obj, "multiply complete, product has %u bits\n", (uint32)mpz_sizeinbase(rb->prime_product, 2)); rb->savefile = savefile; rb->print_relation = print_relation; /* compute the cutoffs used by the recursion base-case. Large primes have a maximum size specified as input arguments, but numbers that can be passed to the SQUFOF routine are limited to size 2^62 */ rb->lp_cutoff_r = lp_cutoff_r; lp_cutoff_r = MIN(lp_cutoff_r, 0x7fffffff); mp_clear(&rb->lp_cutoff_r2); rb->lp_cutoff_r2.nwords = 1; rb->lp_cutoff_r2.val[0] = lp_cutoff_r; mp_mul_1(&rb->lp_cutoff_r2, lp_cutoff_r, &rb->lp_cutoff_r2); rb->lp_cutoff_a = lp_cutoff_a; lp_cutoff_a = MIN(lp_cutoff_a, 0x7fffffff); mp_clear(&rb->lp_cutoff_a2); rb->lp_cutoff_a2.nwords = 1; rb->lp_cutoff_a2.val[0] = lp_cutoff_a; mp_mul_1(&rb->lp_cutoff_a2, lp_cutoff_a, &rb->lp_cutoff_a2); mp_clear(&rb->max_prime2); rb->max_prime2.nwords = 1; rb->max_prime2.val[0] = max_prime; mp_mul_1(&rb->max_prime2, max_prime, &rb->max_prime2); /* allocate lists for relations and their factors */ rb->target_relations = 500000; rb->num_relations = 0; rb->num_relations_alloc = 1000; rb->relations = (cofactor_t *)xmalloc(rb->num_relations_alloc * sizeof(cofactor_t)); rb->num_factors = 0; rb->num_factors_alloc = 10000; rb->factors = (uint32 *)xmalloc(rb->num_factors_alloc * sizeof(uint32)); }
if (!cli_send_trans(cli, SMBtrans2, NULL, /* Name */ -1, 0, /* fid, flags */ &setup, 1, 0, /* setup, length, max */ param, param_len, 10, /* param, length, max */ NULL, 0, #if 0 /* w2k value. */ MIN(16384,cli->max_xmit) /* data, length, max. */ #else cli->max_xmit /* data, length, max. */ #endif )) { break; }
/** * g2d_copy - copy contents in source buffer to destination buffer. * * @ctx: a pointer to g2d_context structure. * @src: a pointer to g2d_image structure including image and buffer * information to source. * @dst: a pointer to g2d_image structure including image and buffer * information to destination. * @src_x: x start position to source buffer. * @src_y: y start position to source buffer. * @dst_x: x start position to destination buffer. * @dst_y: y start position to destination buffer. * @w: width value to source and destination buffers. * @h: height value to source and destination buffers. */ int g2d_copy(struct g2d_context *ctx, struct g2d_image *src, struct g2d_image *dst, unsigned int src_x, unsigned int src_y, unsigned int dst_x, unsigned dst_y, unsigned int w, unsigned int h) { union g2d_rop4_val rop4; union g2d_point_val pt; unsigned int src_w = 0, src_h = 0, dst_w = 0, dst_h = 0; g2d_add_cmd(ctx, DST_SELECT_REG, G2D_SELECT_MODE_BGCOLOR); g2d_add_cmd(ctx, DST_COLOR_MODE_REG, dst->color_mode); g2d_add_base_addr(ctx, dst, g2d_dst); g2d_add_cmd(ctx, DST_STRIDE_REG, dst->stride); g2d_add_cmd(ctx, SRC_SELECT_REG, G2D_SELECT_MODE_NORMAL); g2d_add_cmd(ctx, SRC_COLOR_MODE_REG, src->color_mode); g2d_add_base_addr(ctx, src, g2d_src); g2d_add_cmd(ctx, SRC_STRIDE_REG, src->stride); src_w = w; src_h = h; if (src_x + src->width > w) src_w = src->width - src_x; if (src_y + src->height > h) src_h = src->height - src_y; dst_w = w; dst_h = w; if (dst_x + dst->width > w) dst_w = dst->width - dst_x; if (dst_y + dst->height > h) dst_h = dst->height - dst_y; w = MIN(src_w, dst_w); h = MIN(src_h, dst_h); if (w <= 0 || h <= 0) { fprintf(stderr, "invalid width or height.\n"); g2d_reset(ctx); return -EINVAL; } pt.val = 0; pt.data.x = src_x; pt.data.y = src_y; g2d_add_cmd(ctx, SRC_LEFT_TOP_REG, pt.val); pt.val = 0; pt.data.x = src_x + w; pt.data.y = src_y + h; g2d_add_cmd(ctx, SRC_RIGHT_BOTTOM_REG, pt.val); pt.val = 0; pt.data.x = dst_x; pt.data.y = dst_y; g2d_add_cmd(ctx, DST_LEFT_TOP_REG, pt.val); pt.val = 0; pt.data.x = dst_x + w; pt.data.y = dst_y + h; g2d_add_cmd(ctx, DST_RIGHT_BOTTOM_REG, pt.val); rop4.val = 0; rop4.data.unmasked_rop3 = G2D_ROP3_SRC; g2d_add_cmd(ctx, ROP4_REG, rop4.val); return g2d_flush(ctx); }
static void print_result(const char *file, const int lineno, int ttype, const char *fmt, va_list va) { char buf[1024]; char *str = buf; int ret, size = sizeof(buf), ssize; const char *str_errno = NULL; const char *res; switch (TTYPE_RESULT(ttype)) { case TPASS: res = "PASS"; break; case TFAIL: res = "FAIL"; break; case TBROK: res = "BROK"; break; case TCONF: res = "CONF"; break; case TWARN: res = "WARN"; break; case TINFO: res = "INFO"; break; default: tst_brk(TBROK, "Invalid ttype value %i", ttype); abort(); } if (ttype & TERRNO) str_errno = tst_strerrno(errno); if (ttype & TTERRNO) str_errno = tst_strerrno(TEST_ERRNO); ret = snprintf(str, size, "%s:%i: ", file, lineno); str += ret; size -= ret; if (tst_color_enabled(STDERR_FILENO)) ret = snprintf(str, size, "%s%s: %s", tst_ttype2color(ttype), res, ANSI_COLOR_RESET); else ret = snprintf(str, size, "%s: ", res); str += ret; size -= ret; ssize = size - 2; ret = vsnprintf(str, size, fmt, va); str += MIN(ret, ssize); size -= MIN(ret, ssize); if (ret >= ssize) { tst_res_(file, lineno, TWARN, "Next message is too long and truncated:"); } else if (str_errno) { ssize = size - 2; ret = snprintf(str, size, ": %s", str_errno); str += MIN(ret, ssize); size -= MIN(ret, ssize); if (ret >= ssize) tst_res_(file, lineno, TWARN, "Next message is too long and truncated:"); } snprintf(str, size, "\n"); fputs(buf, stderr); }
/* * read a string terminated by \r or \n in from a fd * * Created: Sat Dec 12 06:29:58 EST 1992 by avalon * Returns: * 0 - EOF * -1 - error on read * >0 - number of bytes returned (<=num) * After opening a fd, it is necessary to init dgets() by calling it as * dgets(x,y,0); * to mark the buffer as being empty. * * cleaned up by - Dianora aug 7 1997 *argh* */ int dgets(int fd, char *buf, int num) { static char dgbuf[8192]; static char *head = dgbuf, *tail = dgbuf; char *s, *t; int n, nr; /* Sanity checks. */ if (head == tail) *head = '\0'; if (!num) { head = tail = dgbuf; *head = '\0'; return 0; } if (num > sizeof(dgbuf) - 1) num = sizeof(dgbuf) - 1; FOREVER { if (head > dgbuf) { for (nr = tail - head, s = head, t = dgbuf; nr > 0; nr--) *t++ = *s++; tail = t; head = dgbuf; } /* check input buffer for EOL and if present return string. */ if (head < tail && ((s = strchr(head, '\n')) || (s = strchr(head, '\r'))) && s < tail) { n = MIN(s - head + 1, num); /* at least 1 byte */ memcpy(buf, head, n); head += n; if (head == tail) head = tail = dgbuf; return n; } if (tail - head >= num) { /* dgets buf is big enough */ n = num; memcpy(buf, head, n); head += n; if (head == tail) head = tail = dgbuf; return n; } n = sizeof(dgbuf) - (tail - dgbuf) - 1; nr = read(fd, tail, n); if (nr == -1) { head = tail = dgbuf; return -1; } if (!nr) { if (tail > head) { n = MIN(tail - head, num); memcpy(buf, head, n); head += n; if (head == tail) head = tail = dgbuf; return n; } head = tail = dgbuf; return 0; } tail += nr; *tail = '\0'; for (t = head; (s = strchr(t, '\n'));) { if ((s > head) && (s > dgbuf)) { t = s - 1; for (nr = 0; *t == '\\'; nr++) t--; if (nr & 1) { t = s + 1; s--; nr = tail - t; while (nr--) *s++ = *t++; tail -= 2; *tail = '\0'; } else s++; } else s++; t = s; } *tail = '\0'; } }
doube scale (double x) { return MIN ( SCALE * x, 0.); }
/* main flight mode dependent update code */ void Plane::update_flight_mode(void) { enum FlightMode effective_mode = control_mode; if (control_mode == AUTO && g.auto_fbw_steer == 42) { effective_mode = FLY_BY_WIRE_A; } if (effective_mode != AUTO) { // hold_course is only used in takeoff and landing steer_state.hold_course_cd = -1; } // ensure we are fly-forward when we are flying as a pure fixed // wing aircraft. This helps the EKF produce better state // estimates as it can make stronger assumptions if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) { ahrs.set_fly_forward(false); } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { ahrs.set_fly_forward(landing.is_flying_forward()); } else { ahrs.set_fly_forward(true); } switch (effective_mode) { case AUTO: handle_auto_mode(); break; case AVOID_ADSB: case GUIDED: if (auto_state.vtol_loiter && quadplane.available()) { quadplane.guided_update(); break; } FALLTHROUGH; case RTL: case LOITER: calc_nav_roll(); calc_nav_pitch(); calc_throttle(); break; case TRAINING: { training_manual_roll = false; training_manual_pitch = false; update_load_factor(); // if the roll is past the set roll limit, then // we set target roll to the limit if (ahrs.roll_sensor >= roll_limit_cd) { nav_roll_cd = roll_limit_cd; } else if (ahrs.roll_sensor <= -roll_limit_cd) { nav_roll_cd = -roll_limit_cd; } else { training_manual_roll = true; nav_roll_cd = 0; } // if the pitch is past the set pitch limits, then // we set target pitch to the limit if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) { nav_pitch_cd = aparm.pitch_limit_max_cd; } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) { nav_pitch_cd = pitch_limit_min_cd; } else { training_manual_pitch = true; nav_pitch_cd = 0; } if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } break; } case ACRO: { // handle locked/unlocked control if (acro_state.locked_roll) { nav_roll_cd = acro_state.locked_roll_err; } else { nav_roll_cd = ahrs.roll_sensor; } if (acro_state.locked_pitch) { nav_pitch_cd = acro_state.locked_pitch_cd; } else { nav_pitch_cd = ahrs.pitch_sensor; } break; } case AUTOTUNE: case FLY_BY_WIRE_A: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; } else { nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); } adjust_nav_pitch_throttle(); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) { // FBWA failsafe glide nav_roll_cd = 0; nav_pitch_cd = 0; SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } if (g.fbwa_tdrag_chan > 0) { // check for the user enabling FBWA taildrag takeoff mode bool tdrag_mode = (RC_Channels::get_radio_in(g.fbwa_tdrag_chan-1) > 1700); if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) { if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) { auto_state.fbwa_tdrag_takeoff_mode = true; gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); } } } break; } case FLY_BY_WIRE_B: // Thanks to Yury MonZon for the altitude limit code! nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); update_fbwb_speed_height(); break; case CRUISE: /* in CRUISE mode we use the navigation code to control roll when heading is locked. Heading becomes unlocked on any aileron or rudder input */ if (channel_roll->get_control_in() != 0 || channel_rudder->get_control_in() != 0) { cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; } if (!cruise_state.locked_heading) { nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); } else { calc_nav_roll(); } update_fbwb_speed_height(); break; case STABILIZE: nav_roll_cd = 0; nav_pitch_cd = 0; // throttle is passthrough break; case CIRCLE: // we have no GPS installed and have lost radio contact // or we just want to fly around in a gentle circle w/o GPS, // holding altitude at the altitude we set when we // switched into the mode nav_roll_cd = roll_limit_cd / 3; update_load_factor(); calc_nav_pitch(); calc_throttle(); break; case MANUAL: SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz()); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz()); steering_control.steering = steering_control.rudder = channel_rudder->get_control_in_zero_dz(); break; case QSTABILIZE: case QHOVER: case QLOITER: case QLAND: case QRTL: { // set nav_roll and nav_pitch using sticks int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max); nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); float pitch_input = channel_pitch->norm_input(); // Scale from normalized input [-1,1] to centidegrees if (quadplane.tailsitter_active()) { // For tailsitters, the pitch range is symmetrical: [-Q_ANGLE_MAX,Q_ANGLE_MAX] nav_pitch_cd = pitch_input * quadplane.aparm.angle_max; } else { // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX if (pitch_input > 0) { nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max); } else { nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max); } nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); } break; } case INITIALISING: // handled elsewhere break; } }
bool BodyPair2DSW::setup(float p_step) { //cannot collide if ((A->get_layer_mask()&B->get_layer_mask())==0 || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) { collided=false; return false; } //use local A coordinates to avoid numerical issues on collision detection offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); _validate_contacts(); Vector2 offset_A = A->get_transform().get_origin(); Matrix32 xform_Au = A->get_transform().untranslated(); Matrix32 xform_A = xform_Au * A->get_shape_transform(shape_A); Matrix32 xform_Bu = B->get_transform(); xform_Bu.elements[2]-=A->get_transform().get_origin(); Matrix32 xform_B = xform_Bu * B->get_shape_transform(shape_B); Shape2DSW *shape_A_ptr=A->get_shape(shape_A); Shape2DSW *shape_B_ptr=B->get_shape(shape_B); Vector2 motion_A,motion_B; if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) { motion_A=A->get_motion(); } if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_SHAPE) { motion_B=B->get_motion(); } //faster to set than to check.. collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_B,_add_contact,this,&sep_axis); if (!collided) { //test ccd (currently just a raycast) if (A->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && A->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) { if (_test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B)) collided=true; } if (B->get_continuous_collision_detection_mode()==Physics2DServer::CCD_MODE_CAST_RAY && B->get_mode()>Physics2DServer::BODY_MODE_KINEMATIC) { if (_test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A,true)) collided=true; } if (!collided) return false; } real_t max_penetration = space->get_contact_max_allowed_penetration(); float bias = 0.3f; if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias()==0) bias=shape_B_ptr->get_custom_bias(); else if (shape_B_ptr->get_custom_bias()==0) bias=shape_A_ptr->get_custom_bias(); else bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5; } cc=0; real_t inv_dt = 1.0/p_step; for (int i = 0; i < contact_count; i++) { Contact& c = contacts[i]; Vector2 global_A = xform_Au.xform(c.local_A); Vector2 global_B = xform_Bu.xform(c.local_B); real_t depth = c.normal.dot(global_A - global_B); if (depth<=0 || !c.reused) { c.active=false; continue; } c.active=true; int gather_A = A->can_report_contacts(); int gather_B = B->can_report_contacts(); c.rA = global_A; c.rB = global_B-offset_B; if (gather_A | gather_B) { //Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); global_A+=offset_A; global_B+=offset_A; if (gather_A) { Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB+B->get_linear_velocity()); } if (gather_B) { Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x ); B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA+A->get_linear_velocity()); } } if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC && B->get_mode()<=Physics2DServer::BODY_MODE_KINEMATIC)) { c.active=false; collided=false; continue; } // Precompute normal mass, tangent mass, and bias. real_t rnA = c.rA.dot(c.normal); real_t rnB = c.rB.dot(c.normal); real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); kNormal += A->get_inv_inertia() * (c.rA.dot(c.rA) - rnA * rnA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rnB * rnB); c.mass_normal = 1.0f / kNormal; Vector2 tangent = c.normal.tangent(); real_t rtA = c.rA.dot(tangent); real_t rtB = c.rB.dot(tangent); real_t kTangent = A->get_inv_mass() + B->get_inv_mass(); kTangent += A->get_inv_inertia() * (c.rA.dot(c.rA) - rtA * rtA) + B->get_inv_inertia() * (c.rB.dot(c.rB) - rtB * rtB); c.mass_tangent = 1.0f / kTangent; c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); c.depth=depth; //c.acc_bias_impulse=0; #ifdef ACCUMULATE_IMPULSES { // Apply normal + friction impulse Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent; A->apply_impulse(c.rA,-P); B->apply_impulse(c.rB, P); } #endif c.bounce=MAX(A->get_bounce(),B->get_bounce()); if (c.bounce) { Vector2 crA( -A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x ); Vector2 crB( -B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x ); Vector2 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; c.bounce = c.bounce * dv.dot(c.normal); } } return true; }
int pb_sub(pb_poly *a, pb_poly *b, pb_poly *c) { int neg, err, x, y, z, characteristic; pb_poly *tmp; /* grow c to be the max size */ y = MAX(a->used, b->used); if (c->alloc < y) { if ((err = pb_grow(c, y)) != MP_OKAY) { return err; } } /* do we need to concern char */ characteristic = mp_iszero(&(c->characteristic)); /* sub the terms */ z = MIN(a->used, b->used); for (x = 0; x < z; x++) { if ((err = mp_sub(&(a->terms[x]), &(b->terms[x]), &(c->terms[x]))) != MP_OKAY) { return err; } if (characteristic == MP_NO) { if ((err = mp_mod(&(c->terms[x]), &(c->characteristic), &(c->terms[x]))) != MP_OKAY) { return err; } } } /* excess digits? */ if (y != z) { if (a->used == y) { tmp = a; neg = 0; } else { tmp = b; neg = 1; } for (x = z; x < y; x++) { if (characteristic == MP_NO) { if ((err = mp_mod(&(tmp->terms[x]), &(c->characteristic), &(c->terms[x]))) != MP_OKAY) { return err; } if (neg) { if ((err = mp_sub(&(c->characteristic), &(c->terms[x]), &(c->terms[x]))) != MP_OKAY) { return err; } } } else { if (neg) { if ((err = mp_neg(&(tmp->terms[x]), &(c->terms[x]))) != MP_OKAY) { return err; } } else { if ((err = mp_copy(&(tmp->terms[x]), &(c->terms[x]))) != MP_OKAY) { return err; } } } } } /* zero excess */ for (x = y; x < c->used; x++) { mp_zero(&(c->terms[x])); } c->used = y; pb_clamp(c); return MP_OKAY; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { if( nrhs != 2 ) mexErrMsgTxt("Exactly one string input argument and one cell array argument required."); if( nlhs != 0 ) mexErrMsgTxt("No output arguments required."); char* path = getString(prhs[0]); std::vector<region_container*> regions; if (!mxIsCell(prhs[1])) mexErrMsgTxt("Second argument must be a cell array"); int length = MAX(mxGetM(prhs[1]), mxGetN(prhs[1])); if ( MIN(mxGetM(prhs[1]), mxGetN(prhs[1])) != 1 ) mexErrMsgTxt("Cell array must be a vector"); for (int i = 0; i < length; i++) { mxArray* val = mxGetCell (prhs[1], i); double *d = (double*) mxGetPr(val); region_container* region = NULL; int l = MAX(mxGetM(val), mxGetN(val)); if (MIN(mxGetM(val), mxGetN(val)) == 1) { if (l == 1) { region = region_create_special(d[0]); } else if (l == 4) { region = region_create_rectangle(d[0], d[1], d[2], d[3]); } else if (l > 5 && l % 2 == 0) { region = region_create_polygon(l / 2); for (int j = 0; j < l / 2; j++) { region->data.polygon.x[j] = d[j * 2]; region->data.polygon.y[j] = d[j * 2 + 1]; } } } if (region) { regions.push_back(region); } else { char message[128]; sprintf(message, "Not a valid region at position %d, skipping", i+1); mexWarnMsgTxt(message); } } std::ofstream ofs; ofs.open (path, std::ofstream::out | std::ofstream::app); if (ofs.is_open()) { for (int i = 0; i < regions.size(); i++) { region_container* region = regions[i]; char * tmp = region_string(region); if (tmp) { ofs << tmp << "\n"; free(tmp); } region_release(®ion); } } else { free(path); mexErrMsgTxt("Unable to open file for writing."); } ofs.close(); free(path); }
// wahbm compressed in place //{{{ uint32_t wah_compressed_in_place_or(uint32_t *r_wah, uint32_t wah_compressed_in_place_or(uint32_t *r_wah, uint32_t r_wah_size, uint32_t *wah, uint32_t wah_size) { uint32_t wah_i, wah_v, wah_fill_size, wah_fill_value, r_wah_i, r_wah_v, r_wah_fill_size, r_wah_fill_value, end, num_words; r_wah_i = 0; for (wah_i = 0; wah_i < wah_size; ++wah_i) { wah_v = wah[wah_i]; r_wah_v = r_wah[r_wah_i]; if (wah_v == 0x80000000) abort(); if (r_wah_v == 0x80000000) abort(); if (wah_v >= 0x80000000) { // wah_v is a fill wah_fill_value = (wah_v >> 30) & 1; wah_fill_size = (wah_v & 0x3fffffff); while (wah_fill_size > 0) { if (r_wah_v >= 0x80000000) { // r_wah is a fill /* fprintf(stderr, "%u:%s\t%u:%s\n", wah_i,int_to_binary(wah_v), r_wah_i,int_to_binary(r_wah_v)); */ r_wah_fill_value = (r_wah_v >> 30) & 1; r_wah_fill_size = (r_wah_v & 0x3fffffff); // make a new fill based on the smaller one num_words = MIN(wah_fill_size, r_wah_fill_size); if (num_words > 1) { r_wah[r_wah_i] = (1 << 31) + ((r_wah_fill_value | wah_fill_value) << 30) + num_words; } else { if ((r_wah_fill_value | wah_fill_value) == 1) r_wah[r_wah_i] = 0x7fffffff; else r_wah[r_wah_i] = 0; } r_wah_fill_size -= num_words; wah_fill_size -= num_words; // save any values left on the end of r_wah run if (r_wah_fill_size > 0) { if (r_wah_fill_size == 1) { // we no longer have a fill, write a literal if (r_wah_fill_value == 1) { //all ones r_wah[r_wah_i + num_words] = 0x7fffffff; //fprintf(stderr,"2\n"); } else { // all zeros r_wah[r_wah_i + num_words] = 0; //fprintf(stderr,"3\n"); } } else { // we still have a fill, write it r_wah[r_wah_i + num_words] = (1 << 31) + (r_wah_fill_value << 30) + r_wah_fill_size; //fprintf(stderr,"4\n"); } } r_wah_i += num_words; } else { // r_wah is a literal if (wah_fill_value == 1) r_wah[r_wah_i] = 0x7fffffff; r_wah_i += 1; wah_fill_size -= 1; } if (r_wah_i < r_wah_size) r_wah_v = r_wah[r_wah_i]; if (r_wah_v == 0x80000000) abort(); }
int zgehrd_(int *n, int *ilo, int *ihi, doublecomplex *a, int *lda, doublecomplex *tau, doublecomplex * work, int *lwork, int *info) { /* System generated locals */ int a_dim1, a_offset, i__1, i__2, i__3, i__4; doublecomplex z__1; /* Local variables */ int i__, j; doublecomplex t[4160] /* was [65][64] */; int ib; doublecomplex ei; int nb, nh, nx, iws, nbmin, iinfo; extern int zgemm_(char *, char *, int *, int *, int *, doublecomplex *, doublecomplex *, int *, doublecomplex *, int *, doublecomplex *, doublecomplex *, int *), ztrmm_(char *, char *, char *, char *, int *, int *, doublecomplex *, doublecomplex *, int * , doublecomplex *, int *), zaxpy_(int *, doublecomplex *, doublecomplex *, int *, doublecomplex *, int *), zgehd2_(int *, int *, int *, doublecomplex *, int *, doublecomplex *, doublecomplex *, int *), zlahr2_(int *, int *, int *, doublecomplex *, int *, doublecomplex *, doublecomplex *, int *, doublecomplex *, int *), xerbla_( char *, int *); extern int ilaenv_(int *, char *, char *, int *, int *, int *, int *); extern int zlarfb_(char *, char *, char *, char *, int *, int *, int *, doublecomplex *, int *, doublecomplex *, int *, doublecomplex *, int *, doublecomplex *, int *); int ldwork, lwkopt; int lquery; /* -- LAPACK routine (version 3.2) -- */ /* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */ /* November 2006 */ /* .. Scalar Arguments .. */ /* .. */ /* .. Array Arguments .. */ /* .. */ /* Purpose */ /* ======= */ /* ZGEHRD reduces a complex general matrix A to upper Hessenberg form H by */ /* an unitary similarity transformation: Q' * A * Q = H . */ /* Arguments */ /* ========= */ /* N (input) INTEGER */ /* The order of the matrix A. N >= 0. */ /* ILO (input) INTEGER */ /* IHI (input) INTEGER */ /* It is assumed that A is already upper triangular in rows */ /* and columns 1:ILO-1 and IHI+1:N. ILO and IHI are normally */ /* set by a previous call to ZGEBAL; otherwise they should be */ /* set to 1 and N respectively. See Further Details. */ /* 1 <= ILO <= IHI <= N, if N > 0; ILO=1 and IHI=0, if N=0. */ /* A (input/output) COMPLEX*16 array, dimension (LDA,N) */ /* On entry, the N-by-N general matrix to be reduced. */ /* On exit, the upper triangle and the first subdiagonal of A */ /* are overwritten with the upper Hessenberg matrix H, and the */ /* elements below the first subdiagonal, with the array TAU, */ /* represent the unitary matrix Q as a product of elementary */ /* reflectors. See Further Details. */ /* LDA (input) INTEGER */ /* The leading dimension of the array A. LDA >= MAX(1,N). */ /* TAU (output) COMPLEX*16 array, dimension (N-1) */ /* The scalar factors of the elementary reflectors (see Further */ /* Details). Elements 1:ILO-1 and IHI:N-1 of TAU are set to */ /* zero. */ /* WORK (workspace/output) COMPLEX*16 array, dimension (LWORK) */ /* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */ /* LWORK (input) INTEGER */ /* The length of the array WORK. LWORK >= MAX(1,N). */ /* For optimum performance LWORK >= N*NB, where NB is the */ /* optimal blocksize. */ /* If LWORK = -1, then a workspace query is assumed; the routine */ /* only calculates the optimal size of the WORK array, returns */ /* this value as the first entry of the WORK array, and no error */ /* message related to LWORK is issued by XERBLA. */ /* INFO (output) INTEGER */ /* = 0: successful exit */ /* < 0: if INFO = -i, the i-th argument had an illegal value. */ /* Further Details */ /* =============== */ /* The matrix Q is represented as a product of (ihi-ilo) elementary */ /* reflectors */ /* Q = H(ilo) H(ilo+1) . . . H(ihi-1). */ /* Each H(i) has the form */ /* H(i) = I - tau * v * v' */ /* where tau is a complex scalar, and v is a complex vector with */ /* v(1:i) = 0, v(i+1) = 1 and v(ihi+1:n) = 0; v(i+2:ihi) is stored on */ /* exit in A(i+2:ihi,i), and tau in TAU(i). */ /* The contents of A are illustrated by the following example, with */ /* n = 7, ilo = 2 and ihi = 6: */ /* on entry, on exit, */ /* ( a a a a a a a ) ( a a h h h h a ) */ /* ( a a a a a a ) ( a h h h h a ) */ /* ( a a a a a a ) ( h h h h h h ) */ /* ( a a a a a a ) ( v2 h h h h h ) */ /* ( a a a a a a ) ( v2 v3 h h h h ) */ /* ( a a a a a a ) ( v2 v3 v4 h h h ) */ /* ( a ) ( a ) */ /* where a denotes an element of the original matrix A, h denotes a */ /* modified element of the upper Hessenberg matrix H, and vi denotes an */ /* element of the vector defining H(i). */ /* This file is a slight modification of LAPACK-3.0's ZGEHRD */ /* subroutine incorporating improvements proposed by Quintana-Orti and */ /* Van de Geijn (2005). */ /* ===================================================================== */ /* .. Parameters .. */ /* .. */ /* .. Local Scalars .. */ /* .. */ /* .. Local Arrays .. */ /* .. */ /* .. External Subroutines .. */ /* .. */ /* .. Intrinsic Functions .. */ /* .. */ /* .. External Functions .. */ /* .. */ /* .. Executable Statements .. */ /* Test the input parameters */ /* Parameter adjustments */ a_dim1 = *lda; a_offset = 1 + a_dim1; a -= a_offset; --tau; --work; /* Function Body */ *info = 0; /* Computing MIN */ i__1 = 64, i__2 = ilaenv_(&c__1, "ZGEHRD", " ", n, ilo, ihi, &c_n1); nb = MIN(i__1,i__2); lwkopt = *n * nb; work[1].r = (double) lwkopt, work[1].i = 0.; lquery = *lwork == -1; if (*n < 0) { *info = -1; } else if (*ilo < 1 || *ilo > MAX(1,*n)) { *info = -2; } else if (*ihi < MIN(*ilo,*n) || *ihi > *n) { *info = -3; } else if (*lda < MAX(1,*n)) { *info = -5; } else if (*lwork < MAX(1,*n) && ! lquery) { *info = -8; } if (*info != 0) { i__1 = -(*info); xerbla_("ZGEHRD", &i__1); return 0; } else if (lquery) { return 0; } /* Set elements 1:ILO-1 and IHI:N-1 of TAU to zero */ i__1 = *ilo - 1; for (i__ = 1; i__ <= i__1; ++i__) { i__2 = i__; tau[i__2].r = 0., tau[i__2].i = 0.; /* L10: */ } i__1 = *n - 1; for (i__ = MAX(1,*ihi); i__ <= i__1; ++i__) { i__2 = i__; tau[i__2].r = 0., tau[i__2].i = 0.; /* L20: */ } /* Quick return if possible */ nh = *ihi - *ilo + 1; if (nh <= 1) { work[1].r = 1., work[1].i = 0.; return 0; } /* Determine the block size */ /* Computing MIN */ i__1 = 64, i__2 = ilaenv_(&c__1, "ZGEHRD", " ", n, ilo, ihi, &c_n1); nb = MIN(i__1,i__2); nbmin = 2; iws = 1; if (nb > 1 && nb < nh) { /* Determine when to cross over from blocked to unblocked code */ /* (last block is always handled by unblocked code) */ /* Computing MAX */ i__1 = nb, i__2 = ilaenv_(&c__3, "ZGEHRD", " ", n, ilo, ihi, &c_n1); nx = MAX(i__1,i__2); if (nx < nh) { /* Determine if workspace is large enough for blocked code */ iws = *n * nb; if (*lwork < iws) { /* Not enough workspace to use optimal NB: determine the */ /* minimum value of NB, and reduce NB or force use of */ /* unblocked code */ /* Computing MAX */ i__1 = 2, i__2 = ilaenv_(&c__2, "ZGEHRD", " ", n, ilo, ihi, & c_n1); nbmin = MAX(i__1,i__2); if (*lwork >= *n * nbmin) { nb = *lwork / *n; } else { nb = 1; } } } } ldwork = *n; if (nb < nbmin || nb >= nh) { /* Use unblocked code below */ i__ = *ilo; } else { /* Use blocked code */ i__1 = *ihi - 1 - nx; i__2 = nb; for (i__ = *ilo; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) { /* Computing MIN */ i__3 = nb, i__4 = *ihi - i__; ib = MIN(i__3,i__4); /* Reduce columns i:i+ib-1 to Hessenberg form, returning the */ /* matrices V and T of the block reflector H = I - V*T*V' */ /* which performs the reduction, and also the matrix Y = A*V*T */ zlahr2_(ihi, &i__, &ib, &a[i__ * a_dim1 + 1], lda, &tau[i__], t, & c__65, &work[1], &ldwork); /* Apply the block reflector H to A(1:ihi,i+ib:ihi) from the */ /* right, computing A := A - Y * V'. V(i+ib,ib-1) must be set */ /* to 1 */ i__3 = i__ + ib + (i__ + ib - 1) * a_dim1; ei.r = a[i__3].r, ei.i = a[i__3].i; i__3 = i__ + ib + (i__ + ib - 1) * a_dim1; a[i__3].r = 1., a[i__3].i = 0.; i__3 = *ihi - i__ - ib + 1; z__1.r = -1., z__1.i = -0.; zgemm_("No transpose", "Conjugate transpose", ihi, &i__3, &ib, & z__1, &work[1], &ldwork, &a[i__ + ib + i__ * a_dim1], lda, &c_b2, &a[(i__ + ib) * a_dim1 + 1], lda); i__3 = i__ + ib + (i__ + ib - 1) * a_dim1; a[i__3].r = ei.r, a[i__3].i = ei.i; /* Apply the block reflector H to A(1:i,i+1:i+ib-1) from the */ /* right */ i__3 = ib - 1; ztrmm_("Right", "Lower", "Conjugate transpose", "Unit", &i__, & i__3, &c_b2, &a[i__ + 1 + i__ * a_dim1], lda, &work[1], & ldwork); i__3 = ib - 2; for (j = 0; j <= i__3; ++j) { z__1.r = -1., z__1.i = -0.; zaxpy_(&i__, &z__1, &work[ldwork * j + 1], &c__1, &a[(i__ + j + 1) * a_dim1 + 1], &c__1); /* L30: */ } /* Apply the block reflector H to A(i+1:ihi,i+ib:n) from the */ /* left */ i__3 = *ihi - i__; i__4 = *n - i__ - ib + 1; zlarfb_("Left", "Conjugate transpose", "Forward", "Columnwise", & i__3, &i__4, &ib, &a[i__ + 1 + i__ * a_dim1], lda, t, & c__65, &a[i__ + 1 + (i__ + ib) * a_dim1], lda, &work[1], & ldwork); /* L40: */ } } /* Use unblocked code to reduce the rest of the matrix */ zgehd2_(n, &i__, ihi, &a[a_offset], lda, &tau[1], &work[1], &iinfo); work[1].r = (double) iws, work[1].i = 0.; return 0; /* End of ZGEHRD */ } /* zgehrd_ */