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_ */
