void gc_init()
{
memset(&gc, 0, sizeof(gc));
gc.feed_rate = DEFAULT_FEED;
select_plane(X_AXIS, Y_AXIS, Z_AXIS);
gc.absolute_mode = true;
}
void gc_init() {
memset(&gc, 0, sizeof(gc));
gc.feed_rate = settings.default_feed_rate/60;
gc.seek_rate = settings.default_seek_rate/60;
select_plane(X_AXIS, Y_AXIS, Z_AXIS);
gc.absolute_mode = true;
}
void gc_init() {
memset(&gc, 0, sizeof(gc));
gc.feed_rate = settings.default_feed_rate;
gc.seek_rate = settings.default_seek_rate;
select_plane(X_AXIS, Y_AXIS, Z_AXIS);
gc.absolute_mode = true;
gc.nominal_laser_intensity = LASER_OFF;
}
void gc_init()
{
memset(&gc, 0, sizeof(gc));
gc.feed_rate = settings.default_feed_rate;
select_plane(X_AXIS, Y_AXIS, Z_AXIS);
gc.absolute_mode = true;
// Load default G54 coordinate system.
if (!(settings_read_coord_data(gc.coord_select,gc.coord_system))) {
report_status_message(STATUS_SETTING_READ_FAIL);
}
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace).
uint8_t gc_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
int radius_mode = false;
uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */
double target[3], offset[3];
double p = 0, r = 0;
int int_value;
clear_vector(target);
clear_vector(offset);
gc.status_code = STATUS_OK;
// Disregard comments and block delete
if (line[0] == '(') { return(gc.status_code); }
if (line[0] == '/') { char_counter++; } // ignore block delete
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
#ifdef __AVR_ATmega328P__
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
#endif
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
case 53: absolute_override = true; break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: case 30: case 60: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
}
if(gc.status_code) { break; }
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
char_counter = 0;
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
if (gc.motion_mode == MOTION_MODE_SEEK) {
gc.seek_rate = unit_converted_value/60;
} else {
gc.feed_rate = unit_converted_value/60; // millimeters pr second
}
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
case 'P': p = value; break;
case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': gc.spindle_speed = value; break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode || absolute_override) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
break;
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
// Update spindle state
spindle_run(gc.spindle_direction, gc.spindle_speed);
// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; // Go home
case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; // Dwell
case NEXT_ACTION_SET_COORDINATE_OFFSET: // Coordinate System offset
mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
break;
case NEXT_ACTION_DEFAULT:
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_SEEK:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false);
break;
case MOTION_MODE_LINEAR:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
break;
#ifdef __AVR_ATmega328P__
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
if (radius_mode) {
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
// real CNC, and thus - for practical reasons - we will terminate promptly:
if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) { h_x2_div_d = -h_x2_div_d; }
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2;
offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
}
/*
This segment sets up an clockwise or counterclockwise arc from the current position to the target position around
the center designated by the offset vector. All theta-values measured in radians of deviance from the positive
y-axis.
| <- theta == 0
* * *
* *
* *
* O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
* /
C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*/
// calculate the theta (angle) of the current point
double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]);
// calculate the theta (angle) of the target point
double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0],
target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]);
// ensure that the difference is positive so that we have clockwise travel
if (theta_end < theta_start) { theta_end += 2*M_PI; }
double angular_travel = theta_end-theta_start;
// Invert angular motion if the g-code wanted a counterclockwise arc
if (gc.motion_mode == MOTION_MODE_CCW_ARC) {
angular_travel = angular_travel-2*M_PI;
}
// Find the radius
double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
// Calculate the motion along the depth axis of the helix
double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2];
// Trace the arc
mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
gc.position);
// Finish off with a line to make sure we arrive exactly where we think we are
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
break;
#endif
}
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[];
return(gc.status_code);
}
typename EMSubpixelCorrelatorView<ImagePixelT>::prerasterize_type
EMSubpixelCorrelatorView<ImagePixelT>::prerasterize(BBox2i const& bbox) const {
vw_out(InfoMessage, "stereo") << "EMSubpixelCorrelatorView: rasterizing image block " << bbox << ".\n";
// Find the range of disparity values for this patch.
// int num_good; // not used
BBox2i search_range;
try {
search_range = get_disparity_range(crop(m_course_disparity, bbox));
}
catch (const std::exception& e) {
search_range = BBox2i();
}
#ifdef USE_GRAPHICS
ImageWindow window;
if(debug_level >= 0) {
window = vw_create_window("disparity");
}
#endif
// The area in the right image that we'll be searching is
// determined by the bbox of the left image plus the search
// range.
BBox2i left_crop_bbox(bbox);
BBox2i right_crop_bbox(bbox.min() + search_range.min(),
bbox.max() + search_range.max());
// The correlator requires the images to be the same size. The
// search bbox will always be larger than the given left image
// bbox, so we just make the left bbox the same size as the
// right bbox.
left_crop_bbox.max() = left_crop_bbox.min() + Vector2i(right_crop_bbox.width(), right_crop_bbox.height());
// Finally, we must adjust both bounding boxes to account for
// the size of the kernel itself.
right_crop_bbox.min() -= Vector2i(m_kernel_size[0], m_kernel_size[1]);
right_crop_bbox.max() += Vector2i(m_kernel_size[0], m_kernel_size[1]);
left_crop_bbox.min() -= Vector2i(m_kernel_size[0], m_kernel_size[1]);
left_crop_bbox.max() += Vector2i(m_kernel_size[0], m_kernel_size[1]);
// We crop the images to the expanded bounding box and edge
// extend in case the new bbox extends past the image bounds.
ImageView<ImagePixelT> left_image_patch, right_image_patch;
ImageView<disparity_pixel> disparity_map_patch_in;
ImageView<result_type> disparity_map_patch_out;
left_image_patch = crop(edge_extend(m_left_image, ZeroEdgeExtension()),
left_crop_bbox);
right_image_patch = crop(edge_extend(m_right_image, ZeroEdgeExtension()),
right_crop_bbox);
disparity_map_patch_in = crop(edge_extend(m_course_disparity, ZeroEdgeExtension()),
left_crop_bbox);
disparity_map_patch_out.set_size(disparity_map_patch_in.cols(), disparity_map_patch_in.rows());
// Adjust the disparities to be relative to the cropped
// image pixel locations
for (int v = 0; v < disparity_map_patch_in.rows(); ++v) {
for (int u = 0; u < disparity_map_patch_in.cols(); ++u) {
if (disparity_map_patch_in(u,v).valid()) {
disparity_map_patch_in(u,v).child().x() -= search_range.min().x();
disparity_map_patch_in(u,v).child().y() -= search_range.min().y();
}
}
}
double blur_sigma_progressive = .5; // 3*sigma = 1.5 pixels
// create the pyramid first
std::vector<ImageView<ImagePixelT> > left_pyramid(pyramid_levels), right_pyramid(pyramid_levels);
std::vector<BBox2i> regions_of_interest(pyramid_levels);
std::vector<ImageView<Matrix2x2> > warps(pyramid_levels);
std::vector<ImageView<disparity_pixel> > disparity_map_pyramid(pyramid_levels);
// initialize the pyramid at level 0
left_pyramid[0] = channels_to_planes(left_image_patch);
right_pyramid[0] = channels_to_planes(right_image_patch);
disparity_map_pyramid[0] = disparity_map_patch_in;
regions_of_interest[0] = BBox2i(m_kernel_size[0], m_kernel_size[1],
bbox.width(),bbox.height());
// downsample the disparity map and the image pair to initialize the intermediate levels
for(int i = 1; i < pyramid_levels; i++) {
left_pyramid[i] = subsample(gaussian_filter(left_pyramid[i-1], blur_sigma_progressive), 2);
right_pyramid[i] = subsample(gaussian_filter(right_pyramid[i-1], blur_sigma_progressive), 2);
disparity_map_pyramid[i] = detail::subsample_disp_map_by_two(disparity_map_pyramid[i-1]);
regions_of_interest[i] = BBox2i(regions_of_interest[i-1].min()/2, regions_of_interest[i-1].max()/2);
}
// initialize warps at the lowest resolution level
warps[pyramid_levels-1].set_size(left_pyramid[pyramid_levels-1].cols(),
left_pyramid[pyramid_levels-1].rows());
for(int y = 0; y < warps[pyramid_levels-1].rows(); y++) {
for(int x = 0; x < warps[pyramid_levels-1].cols(); x++) {
warps[pyramid_levels-1](x, y).set_identity();
}
}
#ifdef USE_GRAPHICS
vw_initialize_graphics(0, NULL);
if(debug_level >= 0) {
for(int i = 0; i < pyramid_levels; i++) {
vw_show_image(window, left_pyramid[i]);
usleep((int)(.2*1000*1000));
}
}
#endif
// go up the pyramid; first run refinement, then upsample result for the next level
for(int i = pyramid_levels-1; i >=0; i--) {
vw_out() << "processing pyramid level "
<< i << " of " << pyramid_levels-1 << std::endl;
if(debug_level >= 0) {
std::stringstream stream;
stream << "pyramid_level_" << i << ".tif";
write_image(stream.str(), disparity_map_pyramid[i]);
}
ImageView<ImagePixelT> process_left_image = left_pyramid[i];
ImageView<ImagePixelT> process_right_image = right_pyramid[i];
if(i > 0) { // in this case take refine the upsampled disparity map from the previous level,
// and upsample for the next level
m_subpixel_refine(edge_extend(process_left_image, ZeroEdgeExtension()), edge_extend(process_right_image, ZeroEdgeExtension()),
disparity_map_pyramid[i], disparity_map_pyramid[i], warps[i],
regions_of_interest[i], false, debug_level == i);
// upsample the warps and the refined map for the next level of processing
int up_width = left_pyramid[i-1].cols();
int up_height = left_pyramid[i-1].rows();
warps[i-1] = copy(resize(warps[i], up_width , up_height, ConstantEdgeExtension(), NearestPixelInterpolation())); //upsample affine transforms
disparity_map_pyramid[i-1] = copy(detail::upsample_disp_map_by_two(disparity_map_pyramid[i], up_width, up_height));
}
else { // here there is no next level so we refine directly to the output patch
m_subpixel_refine(edge_extend(process_left_image, ZeroEdgeExtension()), edge_extend(process_right_image, ZeroEdgeExtension()),
disparity_map_pyramid[i], disparity_map_patch_out, warps[i],
regions_of_interest[i], true, debug_level == i);
}
}
#ifdef USE_GRAPHICS
if(debug_level >= 0) {
vw_show_image(window, .5 + select_plane(channels_to_planes(disparity_map_patch_out)/6., 0));
usleep(10*1000*1000);
}
#endif
// Undo the above adjustment
for (int v = 0; v < disparity_map_patch_out.rows(); ++v) {
for (int u = 0; u < disparity_map_patch_out.cols(); ++u) {
if (disparity_map_patch_out(u,v).valid()) {
disparity_map_patch_out(u,v).child().x() += search_range.min().x();
disparity_map_patch_out(u,v).child().y() += search_range.min().y();
}
}
}
#ifdef USE_GRAPHICS
if(debug_level >= 0 ) {
vw_destroy_window(window);
}
#endif
return crop(disparity_map_patch_out, BBox2i(m_kernel_size[0]-bbox.min().x(),
m_kernel_size[1]-bbox.min().y(),
m_left_image.cols(),
m_left_image.rows()));
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. All units and positions are converted and exported to grbl's
// internal functions in terms of (mm, mm/min) and absolute machine coordinates, respectively.
uint8_t gc_execute_line(char *line)
{
// If in alarm state, don't process. Immediately return with error.
// NOTE: Might not be right place for this, but also prevents $N storing during alarm.
if (sys.state == STATE_ALARM) { return(STATUS_ALARM_LOCK); }
uint8_t char_counter = 0;
char letter;
float value;
int int_value;
uint16_t modal_group_words = 0; // Bitflag variable to track and check modal group words in block
uint8_t axis_words = 0; // Bitflag to track which XYZ(ABC) parameters exist in block
float inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
float target[4], offset[4];
clear_vector(target); // XYZ(ABC) axes parameters.
clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
gc.status_code = STATUS_OK;
/* Pass 1: Commands and set all modes. Check for modal group violations.
NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
uint8_t group_number = MODAL_GROUP_NONE;
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
// Set modal group values
switch(int_value) {
case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break;
case 0: case 1: case 2: case 3: case 80: group_number = MODAL_GROUP_1; break;
case 17: case 18: case 19: group_number = MODAL_GROUP_2; break;
case 90: case 91: group_number = MODAL_GROUP_3; break;
case 93: case 94: group_number = MODAL_GROUP_5; break;
case 20: case 21: group_number = MODAL_GROUP_6; break;
case 54: case 55: case 56: case 57: case 58: case 59: group_number = MODAL_GROUP_12; break;
}
// Set 'G' commands
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: non_modal_action = NON_MODAL_DWELL; break;
case 10: non_modal_action = NON_MODAL_SET_COORDINATE_DATA; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30:
int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
switch(int_value) {
case 280: non_modal_action = NON_MODAL_GO_HOME_0; break;
case 281: non_modal_action = NON_MODAL_SET_HOME_0; break;
case 300: non_modal_action = NON_MODAL_GO_HOME_1; break;
case 301: non_modal_action = NON_MODAL_SET_HOME_1; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 53: absolute_override = true; break;
case 54: case 55: case 56: case 57: case 58: case 59:
gc.coord_select = int_value-54;
break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92:
int_value = trunc(10*value); // Multiply by 10 to pick up G92.1
switch(int_value) {
case 920: non_modal_action = NON_MODAL_SET_COORDINATE_OFFSET; break;
case 921: non_modal_action = NON_MODAL_RESET_COORDINATE_OFFSET; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
// Set modal group values
switch(int_value) {
case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break;
case 3: case 4: case 5: group_number = MODAL_GROUP_7; break;
}
// Set 'M' commands
switch(int_value) {
case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
case 1: break; // Optional stop not supported. Ignore.
case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
#ifdef ENABLE_M7
case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break;
#endif
case 8: gc.coolant_mode = COOLANT_FLOOD_ENABLE; break;
case 9: gc.coolant_mode = COOLANT_DISABLE; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
}
// Check for modal group multiple command violations in the current block
if (group_number) {
if ( bit_istrue(modal_group_words,bit(group_number)) ) {
FAIL(STATUS_MODAL_GROUP_VIOLATION);
} else {
bit_true(modal_group_words,bit(group_number));
}
group_number = MODAL_GROUP_NONE; // Reset for next command.
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
/* Pass 2: Parameters. All units converted according to current block commands. Position
parameters are converted and flagged to indicate a change. These can have multiple connotations
for different commands. Each will be converted to their proper value upon execution. */
float p = 0, r = 0;
uint8_t l = 0;
char_counter = 0;
while(next_statement(&letter, &value, line, &char_counter)) {
switch(letter) {
case 'G': case 'M': case 'N': break; // Ignore command statements and line numbers
case 'F':
if (value <= 0) { FAIL(STATUS_INVALID_STATEMENT); } // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = to_millimeters(value); // seconds per motion for this motion only
} else {
gc.feed_rate = to_millimeters(value); // millimeters per minute
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = to_millimeters(value); break;
case 'L': l = trunc(value); break;
case 'P': p = value; break;
case 'R': r = to_millimeters(value); break;
case 'S':
if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
// TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
// gc.spindle_speed = value;
break;
case 'T':
if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
gc.tool = trunc(value);
break;
case 'X': target[X_AXIS] = to_millimeters(value); bit_true(axis_words,bit(X_AXIS)); break;
case 'Y': target[Y_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Y_AXIS)); break;
case 'Z': target[Z_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Z_AXIS)); break;
case 'C': target[C_AXIS] = to_millimeters(value); bit_true(axis_wors,bit(C_AXIS)); break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
/* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41.
NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency
and simplicity purposes, but this should not affect proper g-code execution. */
// ([F]: Set feed and seek rates.)
// TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When
// max axis speed is installed, the calculation can be performed here, or maybe in the planner.
if (sys.state != STATE_CHECK_MODE) {
// ([M6]: Tool change should be executed here.)
// [M3,M4,M5]: Update spindle state
spindle_run(gc.spindle_direction);
// [*M7,M8,M9]: Update coolant state
coolant_run(gc.coolant_mode);
}
// [G54,G55,...,G59]: Coordinate system selection
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_12)) ) { // Check if called in block
float coord_data[N_AXIS];
if (!(settings_read_coord_data(gc.coord_select,coord_data))) { return(STATUS_SETTING_READ_FAIL); }
memcpy(gc.coord_system,coord_data,sizeof(coord_data));
}
// [G4,G10,G28,G30,G92,G92.1]: Perform dwell, set coordinate system data, homing, or set axis offsets.
// NOTE: These commands are in the same modal group, hence are mutually exclusive. G53 is in this
// modal group and do not effect these actions.
switch (non_modal_action) {
case NON_MODAL_DWELL:
if (p < 0) { // Time cannot be negative.
FAIL(STATUS_INVALID_STATEMENT);
} else {
// Ignore dwell in check gcode modes
if (sys.state != STATE_CHECK_MODE) { mc_dwell(p); }
}
break;
case NON_MODAL_SET_COORDINATE_DATA:
int_value = trunc(p); // Convert p value to int.
if ((l != 2 && l != 20) || (int_value < 0 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ...
FAIL(STATUS_UNSUPPORTED_STATEMENT);
} else if (!axis_words && l==2) { // No axis words.
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (int_value > 0) { int_value--; } // Adjust P1-P6 index to EEPROM coordinate data indexing.
else { int_value = gc.coord_select; } // Index P0 as the active coordinate system
float coord_data[N_AXIS];
if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
uint8_t i;
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
if (l == 20) {
coord_data[i] = gc.position[i]-target[i]; // L20: Update axis current position to target
} else {
coord_data[i] = target[i]; // L2: Update coordinate system axis
}
}
}
settings_write_coord_data(int_value,coord_data);
// Update system coordinate system if currently active.
if (gc.coord_select == int_value) { memcpy(gc.coord_system,coord_data,sizeof(coord_data)); }
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1:
// Move to intermediate position before going home. Obeys current coordinate system and offsets
// and absolute and incremental modes.
if (axis_words) {
// Apply absolute mode coordinate offsets or incremental mode offsets.
uint8_t i;
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if ( bit_istrue(axis_words,bit(i)) ) {
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i];
} else {
target[i] += gc.position[i];
}
} else {
target[i] = gc.position[i];
}
}
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS], settings.default_seek_rate, false);
}
// Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
float coord_data[N_AXIS];
if (non_modal_action == NON_MODAL_GO_HOME_1) {
if (!settings_read_coord_data(SETTING_INDEX_G30 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
} else {
if (!settings_read_coord_data(SETTING_INDEX_G28 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
}
mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], coord_data[C_AXIS], settings.default_seek_rate, false);
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
if (non_modal_action == NON_MODAL_SET_HOME_1) {
settings_write_coord_data(SETTING_INDEX_G30,gc.position);
} else {
settings_write_coord_data(SETTING_INDEX_G28,gc.position);
}
break;
case NON_MODAL_SET_COORDINATE_OFFSET:
if (!axis_words) { // No axis words
FAIL(STATUS_INVALID_STATEMENT);
} else {
// Update axes defined only in block. Offsets current system to defined value. Does not update when
// active coordinate system is selected, but is still active unless G92.1 disables it.
uint8_t i;
for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
}
}
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_RESET_COORDINATE_OFFSET:
clear_vector(gc.coord_offset); // Disable G92 offsets by zeroing offset vector.
break;
}
// [G0,G1,G2,G3,G80]: Perform motion modes.
// NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
// Enter motion modes only if there are axis words or a motion mode command word in the block.
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_1)) || axis_words ) {
// G1,G2,G3 require F word in inverse time mode.
if ( gc.inverse_feed_rate_mode ) {
if (inverse_feed_rate < 0 && gc.motion_mode != MOTION_MODE_CANCEL) {
FAIL(STATUS_INVALID_STATEMENT);
}
}
// Absolute override G53 only valid with G0 and G1 active.
if ( absolute_override && !(gc.motion_mode == MOTION_MODE_SEEK || gc.motion_mode == MOTION_MODE_LINEAR)) {
FAIL(STATUS_INVALID_STATEMENT);
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// Convert all target position data to machine coordinates for executing motion. Apply
// absolute mode coordinate offsets or incremental mode offsets.
// NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
uint8_t i;
for (i=0; i<=3; i++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_istrue(axis_words,bit(i)) ) {
if (!absolute_override) { // Do not update target in absolute override mode
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
} else {
target[i] += gc.position[i]; // Incremental mode
}
}
} else {
target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
}
}
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL:
if (axis_words) { FAIL(STATUS_INVALID_STATEMENT); } // No axis words allowed while active.
break;
case MOTION_MODE_SEEK:
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS], settings.default_seek_rate, false); }
break;
case MOTION_MODE_LINEAR:
// TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
// to check for initial F-word upon startup. Maybe just set to zero upon initialization
// and after an inverse time move and then check for non-zero feed rate each time. This
// should be efficient and effective.
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[C_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); }
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// Check if at least one of the axes of the selected plane has been specified. If in center
// format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) ||
( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) {
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (r != 0) { // Arc Radius Mode
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
// First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
// than d. If so, the sqrt of a negative number is complex and error out.
float h_x2_div_d = 4 * r*r - x*x - y*y;
if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
// Finish computing h_x2_div_d.
h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Arc Center Format Offset Mode
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise);
}
break;
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
}
// M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may
// refill and can only be resumed by the cycle start run-time command.
if (gc.program_flow) {
plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
// If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
// re-enable program flow after pause complete, where cycle start will resume the program.
if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
}
return(gc.status_code);
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed.
uint8_t gc_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t radius_mode = false;
uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */
double target[3], offset[3];
double p = 0, r = 0;
int int_value;
gc.status_code = STATUS_OK;
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
case 53: absolute_override = true; break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: case 30: case 60: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: gc.laser_enable = 1; break;
case 4: gc.laser_enable = 1; break;
case 5: gc.laser_enable = 0; break;
case 112: next_action = NEXT_ACTION_CANCEL; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
}
if(gc.status_code) { break; }
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
char_counter = 0;
clear_vector(target);
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
if (gc.motion_mode == MOTION_MODE_SEEK) {
gc.seek_rate = unit_converted_value;
} else {
gc.feed_rate = unit_converted_value; // millimeters per minute
}
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
case 'P': p = value; break;
case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': gc.nominal_laser_intensity = value; break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode || absolute_override) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
break;
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break;
case NEXT_ACTION_DWELL: mc_dwell(p); break;
case NEXT_ACTION_SET_COORDINATE_OFFSET:
mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
break;
case NEXT_ACTION_CANCEL:
//mc_emergency_stop();
// captain, we have a new target!
//st_get_position(&gc.position[X_AXIS], &gc.position[Y_AXIS], &gc.position[Z_AXIS]);
//mc_set_current_position(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS]);
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false, LASER_OFF);
//return; // totally bail
break;
case NEXT_ACTION_DEFAULT:
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_SEEK:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false, LASER_OFF);
break;
case MOTION_MODE_LINEAR:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
gc.nominal_laser_intensity);
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
if (radius_mode) {
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
// real CNC, and thus - for practical reasons - we will terminate promptly:
if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Offset mode specific computations
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise, gc.nominal_laser_intensity);
break;
}
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[];
return(gc.status_code);
}
/*!
*************************************************************************************
* \brief
* Mode Decision for a macroblock
*************************************************************************************
*/
void encode_one_macroblock_low (Macroblock *currMB)
{
int block, mode, i, j, k, dummy;
char best_pdir;
RD_PARAMS enc_mb;
char best_ref[2] = {0, -1};
int bmcost[5] = {INT_MAX};
double rd_cost = 0, min_rd_cost = 1e30;
int cost = 0;
int min_cost = INT_MAX, cost_direct=0, have_direct=0, i16mode=0;
int intra1 = 0;
int temp_cpb = 0;
int best_transform_flag = 0;
int cost8x8_direct = 0;
short islice = (short) (img->type==I_SLICE);
short bslice = (short) (img->type==B_SLICE);
short pslice = (short) ((img->type==P_SLICE) || (img->type==SP_SLICE));
short intra = (short) (islice || (pslice && img->mb_y==img->mb_y_upd && img->mb_y_upd!=img->mb_y_intra));
int lambda_mf[3];
int pix_x, pix_y;
int prev_mb_nr = FmoGetPreviousMBNr(img->current_mb_nr);
Macroblock* prevMB = (prev_mb_nr >= 0) ? &img->mb_data[prev_mb_nr]:NULL ;
char **ipredmodes = img->ipredmode;
short *allmvs = params->IntraProfile ? NULL: img->all_mv[0][0][0][0][0];
int ****i4p; //for non-RD-opt. mode
imgpel (*curr_mpr)[16] = img->mpr[0];
int tmp_8x8_flag, tmp_no_mbpart;
// Fast Mode Decision
short inter_skip = 0;
if(params->SearchMode == UM_HEX)
{
UMHEX_decide_intrabk_SAD();
}
else if (params->SearchMode == UM_HEX_SIMPLE)
{
smpUMHEX_decide_intrabk_SAD();
}
intra |= RandomIntra (img->current_mb_nr); // Forced Pseudo-Random Intra
//===== Setup Macroblock encoding parameters =====
init_enc_mb_params(currMB, &enc_mb, intra, bslice);
// reset chroma intra predictor to default
currMB->c_ipred_mode = DC_PRED_8;
//===== S T O R E C O D I N G S T A T E =====
//---------------------------------------------------
store_coding_state (currMB, cs_cm);
if (!intra)
{
//===== set direct motion vectors =====
best_mode = 1;
if (bslice)
{
Get_Direct_Motion_Vectors (currMB);
}
if (params->CtxAdptLagrangeMult == 1)
{
get_initial_mb16x16_cost(currMB);
}
//===== MOTION ESTIMATION FOR 16x16, 16x8, 8x16 BLOCKS =====
for (min_cost=INT_MAX, mode=1; mode<4; mode++)
{
bi_pred_me = 0;
img->bi_pred_me[mode]=0;
if (enc_mb.valid[mode] && !inter_skip)
{
for (cost=0, block=0; block<(mode==1?1:2); block++)
{
update_lambda_costs(&enc_mb, lambda_mf);
PartitionMotionSearch (currMB, mode, block, lambda_mf);
//--- set 4x4 block indizes (for getting MV) ---
j = (block==1 && mode==2 ? 2 : 0);
i = (block==1 && mode==3 ? 2 : 0);
//--- get cost and reference frame for List 0 prediction ---
bmcost[LIST_0] = INT_MAX;
list_prediction_cost(currMB, LIST_0, block, mode, enc_mb, bmcost, best_ref);
if (bslice)
{
//--- get cost and reference frame for List 1 prediction ---
bmcost[LIST_1] = INT_MAX;
list_prediction_cost(currMB, LIST_1, block, mode, enc_mb, bmcost, best_ref);
// Compute bipredictive cost between best list 0 and best list 1 references
list_prediction_cost(currMB, BI_PRED, block, mode, enc_mb, bmcost, best_ref);
// Finally, if mode 16x16, compute cost for bipredictive ME vectore
if (params->BiPredMotionEstimation && mode == 1)
{
list_prediction_cost(currMB, BI_PRED_L0, block, mode, enc_mb, bmcost, 0);
list_prediction_cost(currMB, BI_PRED_L1, block, mode, enc_mb, bmcost, 0);
}
else
{
bmcost[BI_PRED_L0] = INT_MAX;
bmcost[BI_PRED_L1] = INT_MAX;
}
// Determine prediction list based on mode cost
determine_prediction_list(mode, bmcost, best_ref, &best_pdir, &cost, &bi_pred_me);
}
else // if (bslice)
{
best_pdir = 0;
cost += bmcost[LIST_0];
}
assign_enc_picture_params(mode, best_pdir, block, enc_mb.list_offset[LIST_0], best_ref[LIST_0], best_ref[LIST_1], bslice);
//----- set reference frame and direction parameters -----
if (mode==3)
{
best8x8l0ref [3][block ] = best8x8l0ref [3][ block+2] = best_ref[LIST_0];
best8x8pdir [3][block ] = best8x8pdir [3][ block+2] = best_pdir;
best8x8l1ref [3][block ] = best8x8l1ref [3][ block+2] = best_ref[LIST_1];
}
else if (mode==2)
{
best8x8l0ref [2][2*block] = best8x8l0ref [2][2*block+1] = best_ref[LIST_0];
best8x8pdir [2][2*block] = best8x8pdir [2][2*block+1] = best_pdir;
best8x8l1ref [2][2*block] = best8x8l1ref [2][2*block+1] = best_ref[LIST_1];
}
else
{
memset(&best8x8l0ref [1][0], best_ref[LIST_0], 4 * sizeof(char));
memset(&best8x8l1ref [1][0], best_ref[LIST_1], 4 * sizeof(char));
best8x8pdir [1][0] = best8x8pdir [1][1] = best8x8pdir [1][2] = best8x8pdir [1][3] = best_pdir;
}
//--- set reference frames and motion vectors ---
if (mode>1 && block==0)
SetRefAndMotionVectors (currMB, block, mode, best_pdir, best_ref[LIST_0], best_ref[LIST_1]);
} // for (block=0; block<(mode==1?1:2); block++)
currMB->luma_transform_size_8x8_flag = 0;
if (params->Transform8x8Mode) //for inter rd-off, set 8x8 to do 8x8 transform
{
SetModesAndRefframeForBlocks(currMB, mode);
currMB->luma_transform_size_8x8_flag = TransformDecision(currMB, -1, &cost);
}
if ((!inter_skip) && (cost < min_cost))
{
best_mode = (short) mode;
min_cost = cost;
best_transform_flag = currMB->luma_transform_size_8x8_flag;
if (params->CtxAdptLagrangeMult == 1)
{
adjust_mb16x16_cost(cost);
}
}
} // if (enc_mb.valid[mode])
} // for (mode=1; mode<4; mode++)
if ((!inter_skip) && enc_mb.valid[P8x8])
{
giRDOpt_B8OnlyFlag = 1;
tr8x8.cost8x8 = INT_MAX;
tr4x4.cost8x8 = INT_MAX;
//===== store coding state of macroblock =====
store_coding_state (currMB, cs_mb);
currMB->all_blk_8x8 = -1;
if (params->Transform8x8Mode)
{
tr8x8.cost8x8 = 0;
//===========================================================
// Check 8x8 partition with transform size 8x8
//===========================================================
//===== LOOP OVER 8x8 SUB-PARTITIONS (Motion Estimation & Mode Decision) =====
for (cost_direct=cbp8x8=cbp_blk8x8=cnt_nonz_8x8=0, block = 0; block < 4; block++)
{
submacroblock_mode_decision(enc_mb, &tr8x8, currMB, cofAC8x8ts[0][block], cofAC8x8ts[1][block], cofAC8x8ts[2][block],
&have_direct, bslice, block, &cost_direct, &cost, &cost8x8_direct, 1);
best8x8mode [block] = tr8x8.part8x8mode [block];
best8x8pdir [P8x8][block] = tr8x8.part8x8pdir [block];
best8x8l0ref[P8x8][block] = tr8x8.part8x8l0ref[block];
best8x8l1ref[P8x8][block] = tr8x8.part8x8l1ref[block];
}
// following params could be added in RD_8x8DATA structure
cbp8_8x8ts = cbp8x8;
cbp_blk8_8x8ts = cbp_blk8x8;
cnt_nonz8_8x8ts = cnt_nonz_8x8;
currMB->luma_transform_size_8x8_flag = 0; //switch to 4x4 transform size
//--- re-set coding state (as it was before 8x8 block coding) ---
//reset_coding_state (currMB, cs_mb);
}// if (params->Transform8x8Mode)
if (params->Transform8x8Mode != 2)
{
tr4x4.cost8x8 = 0;
//=================================================================
// Check 8x8, 8x4, 4x8 and 4x4 partitions with transform size 4x4
//=================================================================
//===== LOOP OVER 8x8 SUB-PARTITIONS (Motion Estimation & Mode Decision) =====
for (cost_direct=cbp8x8=cbp_blk8x8=cnt_nonz_8x8=0, block=0; block<4; block++)
{
submacroblock_mode_decision(enc_mb, &tr4x4, currMB, cofAC8x8[block], cofAC8x8CbCr[0][block], cofAC8x8CbCr[1][block],
&have_direct, bslice, block, &cost_direct, &cost, &cost8x8_direct, 0);
best8x8mode [block] = tr4x4.part8x8mode [block];
best8x8pdir [P8x8][block] = tr4x4.part8x8pdir [block];
best8x8l0ref[P8x8][block] = tr4x4.part8x8l0ref[block];
best8x8l1ref[P8x8][block] = tr4x4.part8x8l1ref[block];
}
//--- re-set coding state (as it was before 8x8 block coding) ---
// reset_coding_state (currMB, cs_mb);
}// if (params->Transform8x8Mode != 2)
//--- re-set coding state (as it was before 8x8 block coding) ---
reset_coding_state (currMB, cs_mb);
// This is not enabled yet since mpr has reverse order.
if (params->RCEnable)
rc_store_diff(img->opix_x, img->opix_y, curr_mpr);
//check cost for P8x8 for non-rdopt mode
if (tr4x4.cost8x8 < min_cost || tr8x8.cost8x8 < min_cost)
{
best_mode = P8x8;
if (params->Transform8x8Mode == 2)
{
min_cost = tr8x8.cost8x8;
currMB->luma_transform_size_8x8_flag=1;
}
else if (params->Transform8x8Mode)
{
if (tr8x8.cost8x8 < tr4x4.cost8x8)
{
min_cost = tr8x8.cost8x8;
currMB->luma_transform_size_8x8_flag=1;
}
else if(tr4x4.cost8x8 < tr8x8.cost8x8)
{
min_cost = tr4x4.cost8x8;
currMB->luma_transform_size_8x8_flag=0;
}
else
{
if (GetBestTransformP8x8() == 0)
{
min_cost = tr4x4.cost8x8;
currMB->luma_transform_size_8x8_flag=0;
}
else
{
min_cost = tr8x8.cost8x8;
currMB->luma_transform_size_8x8_flag=1;
}
}
}
else
{
min_cost = tr4x4.cost8x8;
currMB->luma_transform_size_8x8_flag=0;
}
}// if ((tr4x4.cost8x8 < min_cost || tr8x8.cost8x8 < min_cost))
giRDOpt_B8OnlyFlag = 0;
}
else // if (enc_mb.valid[P8x8])
{
tr4x4.cost8x8 = INT_MAX;
}
// Find a motion vector for the Skip mode
if(pslice)
FindSkipModeMotionVector (currMB);
}
else // if (!intra)
{
min_cost = INT_MAX;
}
//========= C H O O S E B E S T M A C R O B L O C K M O D E =========
//-------------------------------------------------------------------------
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag; //save 8x8_flag
tmp_no_mbpart = currMB->NoMbPartLessThan8x8Flag; //save no-part-less
if ((img->yuv_format != YUV400) && (img->yuv_format != YUV444))
// precompute all chroma intra prediction modes
IntraChromaPrediction(currMB, NULL, NULL, NULL);
if (enc_mb.valid[0] && bslice) // check DIRECT MODE
{
if(have_direct)
{
switch(params->Transform8x8Mode)
{
case 1: // Mixture of 8x8 & 4x4 transform
cost = ((cost8x8_direct < cost_direct) || !(enc_mb.valid[5] && enc_mb.valid[6] && enc_mb.valid[7]))
? cost8x8_direct : cost_direct;
break;
case 2: // 8x8 Transform only
cost = cost8x8_direct;
break;
default: // 4x4 Transform only
cost = cost_direct;
break;
}
}
else
{ //!have_direct
cost = GetDirectCostMB (currMB, bslice);
}
if (cost!=INT_MAX)
{
cost -= (int)floor(16*enc_mb.lambda_md+0.4999);
}
if (cost <= min_cost)
{
if(active_sps->direct_8x8_inference_flag && params->Transform8x8Mode)
{
if(params->Transform8x8Mode==2)
currMB->luma_transform_size_8x8_flag=1;
else
{
if(cost8x8_direct < cost_direct)
currMB->luma_transform_size_8x8_flag=1;
else
currMB->luma_transform_size_8x8_flag=0;
}
}
else
currMB->luma_transform_size_8x8_flag=0;
//Rate control
if (params->RCEnable)
rc_store_diff(img->opix_x, img->opix_y, curr_mpr);
min_cost = cost;
best_mode = 0;
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag;
}
else
{
currMB->luma_transform_size_8x8_flag = tmp_8x8_flag; // restore if not best
currMB->NoMbPartLessThan8x8Flag = tmp_no_mbpart; // restore if not best
}
}
min_rd_cost = (double) min_cost;
if (enc_mb.valid[I8MB]) // check INTRA8x8
{
currMB->luma_transform_size_8x8_flag = 1; // at this point cost will ALWAYS be less than min_cost
currMB->mb_type = I8MB;
temp_cpb = Mode_Decision_for_new_Intra8x8Macroblock (currMB, enc_mb.lambda_md, &rd_cost);
if (rd_cost <= min_rd_cost) //HYU_NOTE. bug fix. 08/15/07
{
currMB->cbp = temp_cpb;
if (img->P444_joined)
{
curr_cbp[0] = cmp_cbp[1];
curr_cbp[1] = cmp_cbp[2];
}
if(enc_mb.valid[I4MB]) //KHHan. bug fix. Oct.15.2007
{
//coeffs
if (params->Transform8x8Mode != 2)
{
i4p=cofAC; cofAC=img->cofAC; img->cofAC=i4p;
}
}
for(j=0; j<MB_BLOCK_SIZE; j++)
{
pix_y = img->pix_y + j;
for(i=0; i<MB_BLOCK_SIZE; i++)
{
pix_x = img->pix_x + i;
temp_imgY[j][i] = enc_picture->imgY[pix_y][pix_x];
}
}
if (img->P444_joined)
{
for(j=0; j<MB_BLOCK_SIZE; j++)
{
pix_y = img->pix_y + j;
for(i=0; i<MB_BLOCK_SIZE; i++)
{
pix_x = img->pix_x + i;
temp_imgU[j][i] = enc_picture->imgUV[0][pix_y][pix_x];
temp_imgV[j][i] = enc_picture->imgUV[1][pix_y][pix_x];
}
}
}
//Rate control
if (params->RCEnable)
rc_store_diff(img->opix_x, img->opix_y, curr_mpr);
min_rd_cost = rd_cost;
best_mode = I8MB;
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag;
}
else
{
currMB->luma_transform_size_8x8_flag = tmp_8x8_flag; // restore if not best
if (img->P444_joined)
{
cmp_cbp[1] = curr_cbp[0];
cmp_cbp[2] = curr_cbp[1];
currMB->cbp |= cmp_cbp[1];
currMB->cbp |= cmp_cbp[2];
cmp_cbp[1] = currMB->cbp;
cmp_cbp[2] = currMB->cbp;
}
}
}
if (enc_mb.valid[I4MB]) // check INTRA4x4
{
currMB->luma_transform_size_8x8_flag = 0;
currMB->mb_type = I4MB;
temp_cpb = Mode_Decision_for_Intra4x4Macroblock (currMB, enc_mb.lambda_md, &rd_cost);
if (rd_cost <= min_rd_cost)
{
currMB->cbp = temp_cpb;
//Rate control
if (params->RCEnable)
rc_store_diff(img->opix_x, img->opix_y, curr_mpr);
min_rd_cost = rd_cost;
best_mode = I4MB;
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag;
}
else
{
currMB->luma_transform_size_8x8_flag = tmp_8x8_flag; // restore if not best
if (img->P444_joined)
{
cmp_cbp[1] = curr_cbp[0];
cmp_cbp[2] = curr_cbp[1];
currMB->cbp |= cmp_cbp[1];
currMB->cbp |= cmp_cbp[2];
cmp_cbp[1] = currMB->cbp;
cmp_cbp[2] = currMB->cbp;
}
//coeffs
i4p=cofAC; cofAC=img->cofAC; img->cofAC=i4p;
}
}
if (enc_mb.valid[I16MB]) // check INTRA16x16
{
currMB->luma_transform_size_8x8_flag = 0;
intrapred_16x16 (currMB, PLANE_Y);
if (img->P444_joined)
{
select_plane(PLANE_U);
intrapred_16x16 (currMB, PLANE_U);
select_plane(PLANE_V);
intrapred_16x16 (currMB, PLANE_V);
select_plane(PLANE_Y);
}
switch(params->FastIntra16x16)
{
case 0:
default:
find_sad_16x16 = find_sad_16x16_JM;
break;
}
rd_cost = find_sad_16x16 (currMB, &i16mode);
if (rd_cost < min_rd_cost)
{
//Rate control
if (params->RCEnable)
rc_store_diff(img->opix_x,img->opix_y,img->mpr_16x16[0][i16mode]);
best_mode = I16MB;
min_rd_cost = rd_cost;
currMB->cbp = pDCT_16x16 (currMB, PLANE_Y, i16mode);
if (img->P444_joined)
{
select_plane(PLANE_U);
cmp_cbp[1] = pDCT_16x16(currMB, PLANE_U, i16mode);
select_plane(PLANE_V);
cmp_cbp[2] = pDCT_16x16(currMB, PLANE_V, i16mode);
select_plane(PLANE_Y);
currMB->cbp |= cmp_cbp[1];
currMB->cbp |= cmp_cbp[2];
cmp_cbp[1] = currMB->cbp;
cmp_cbp[2] = currMB->cbp;
}
}
else
{
currMB->luma_transform_size_8x8_flag = tmp_8x8_flag; // restore
currMB->NoMbPartLessThan8x8Flag = tmp_no_mbpart; // restore
}
}
intra1 = IS_INTRA(currMB);
//===== S E T F I N A L M A C R O B L O C K P A R A M E T E R S ======
//---------------------------------------------------------------------------
{
//===== set parameters for chosen mode =====
SetModesAndRefframeForBlocks (currMB, best_mode);
if (best_mode==P8x8)
{
if (currMB->luma_transform_size_8x8_flag && (cbp8_8x8ts == 0) && params->Transform8x8Mode != 2)
currMB->luma_transform_size_8x8_flag = 0;
SetCoeffAndReconstruction8x8 (currMB);
memset(currMB->intra_pred_modes, DC_PRED, MB_BLOCK_PARTITIONS * sizeof(char));
for (k=0, j = img->block_y; j < img->block_y + BLOCK_MULTIPLE; j++)
memset(&ipredmodes[j][img->block_x], DC_PRED, BLOCK_MULTIPLE * sizeof(char));
}
else
{
//===== set parameters for chosen mode =====
if (best_mode == I8MB)
{
memcpy(currMB->intra_pred_modes,currMB->intra_pred_modes8x8, MB_BLOCK_PARTITIONS * sizeof(char));
for(j = img->block_y; j < img->block_y + BLOCK_MULTIPLE; j++)
memcpy(&img->ipredmode[j][img->block_x],&img->ipredmode8x8[j][img->block_x], BLOCK_MULTIPLE * sizeof(char));
//--- restore reconstruction for 8x8 transform ---
for(j=0; j<MB_BLOCK_SIZE; j++)
{
memcpy(&enc_picture->imgY[img->pix_y + j][img->pix_x],temp_imgY[j], MB_BLOCK_SIZE * sizeof(imgpel));
}
if (img->P444_joined)
{
for(j=0; j<MB_BLOCK_SIZE; j++)
{
memcpy(&enc_picture->imgUV[0][img->pix_y + j][img->pix_x],temp_imgU[j], MB_BLOCK_SIZE * sizeof(imgpel));
memcpy(&enc_picture->imgUV[1][img->pix_y + j][img->pix_x],temp_imgV[j], MB_BLOCK_SIZE * sizeof(imgpel));
}
}
}
if ((best_mode!=I4MB)&&(best_mode != I8MB))
{
memset(currMB->intra_pred_modes,DC_PRED, MB_BLOCK_PARTITIONS * sizeof(char));
for(j = img->block_y; j < img->block_y + BLOCK_MULTIPLE; j++)
memset(&ipredmodes[j][img->block_x],DC_PRED, BLOCK_MULTIPLE * sizeof(char));
if (best_mode!=I16MB)
{
if((best_mode>=1) && (best_mode<=3))
currMB->luma_transform_size_8x8_flag = best_transform_flag;
LumaResidualCoding (currMB);
if (img->P444_joined)
{
if((currMB->cbp==0 && cmp_cbp[1] == 0 && cmp_cbp[2] == 0) &&(best_mode==0))
currMB->luma_transform_size_8x8_flag = 0;
}
else if((currMB->cbp==0)&&(best_mode==0))
currMB->luma_transform_size_8x8_flag = 0;
//Rate control
if (params->RCEnable)
rc_store_diff(img->opix_x,img->opix_y,curr_mpr);
}
}
}
//check luma cbp for transform size flag
if (((currMB->cbp&15) == 0) && !(IS_OLDINTRA(currMB) || currMB->mb_type == I8MB))
currMB->luma_transform_size_8x8_flag = 0;
// precompute all chroma intra prediction modes
if ((img->yuv_format != YUV400) && (img->yuv_format != YUV444))
IntraChromaPrediction(currMB, NULL, NULL, NULL);
img->i16offset = 0;
dummy = 0;
if ((img->yuv_format != YUV400) && (img->yuv_format != YUV444))
ChromaResidualCoding (currMB);
if (best_mode==I16MB)
{
img->i16offset = I16Offset (currMB->cbp, i16mode);
}
SetMotionVectorsMB (currMB, bslice);
//===== check for SKIP mode =====
if(img->P444_joined)
{
if ((pslice) && best_mode==1 && currMB->cbp==0 && cmp_cbp[1] == 0 && cmp_cbp[2] == 0 &&
enc_picture->ref_idx[LIST_0][img->block_y][img->block_x] == 0 &&
enc_picture->mv [LIST_0][img->block_y][img->block_x][0] == allmvs[0] &&
enc_picture->mv [LIST_0][img->block_y][img->block_x][1] == allmvs[1])
{
currMB->mb_type = currMB->b8mode[0] = currMB->b8mode[1] = currMB->b8mode[2] = currMB->b8mode[3] = 0;
currMB->luma_transform_size_8x8_flag = 0;
}
}
else if ((pslice) && best_mode==1 && currMB->cbp==0 &&
enc_picture->ref_idx[LIST_0][img->block_y][img->block_x] == 0 &&
enc_picture->mv [LIST_0][img->block_y][img->block_x][0] == allmvs[0] &&
enc_picture->mv [LIST_0][img->block_y][img->block_x][1] == allmvs[1])
{
currMB->mb_type = currMB->b8mode[0] = currMB->b8mode[1] = currMB->b8mode[2] = currMB->b8mode[3] = 0;
currMB->luma_transform_size_8x8_flag = 0;
}
if (img->MbaffFrameFlag || (params->UseRDOQuant && params->RDOQ_QP_Num > 1))
set_mbaff_parameters(currMB);
}
// Rate control
if(params->RCEnable && params->RCUpdateMode <= MAX_RC_MODE)
rc_store_mad(currMB);
update_qp_cbp(currMB, best_mode);
rdopt->min_rdcost = min_rd_cost;
rdopt->min_dcost = min_rd_cost;
if ( (img->MbaffFrameFlag)
&& (img->current_mb_nr%2)
&& (currMB->mb_type ? 0:((bslice) ? !currMB->cbp:1)) // bottom is skip
&& (prevMB->mb_type ? 0:((bslice) ? !prevMB->cbp:1))
&& !(field_flag_inference(currMB) == enc_mb.curr_mb_field)) // top is skip
{
rdopt->min_rdcost = 1e30; // don't allow coding of a MB pair as skip if wrong inference
}
//===== Decide if this MB will restrict the reference frames =====
if (params->RestrictRef)
update_refresh_map(intra, intra1, currMB);
if(params->SearchMode == UM_HEX)
{
UMHEX_skip_intrabk_SAD(best_mode, listXsize[enc_mb.list_offset[LIST_0]]);
}
else if(params->SearchMode == UM_HEX_SIMPLE)
{
smpUMHEX_skip_intrabk_SAD(best_mode, listXsize[enc_mb.list_offset[LIST_0]]);
}
//--- constrain intra prediction ---
if(params->UseConstrainedIntraPred && (img->type==P_SLICE || img->type==B_SLICE))
{
img->intra_block[img->current_mb_nr] = IS_INTRA(currMB);
}
}
/*!
*************************************************************************************
* \brief
* Mode Decision for a macroblock
*************************************************************************************
*/
void encode_one_macroblock_low (Macroblock *currMB)
{
Slice *currSlice = currMB->p_slice;
RDOPTStructure *p_RDO = currSlice->p_RDO;
ImageParameters *p_Img = currMB->p_Img;
InputParameters *p_Inp = currMB->p_Inp;
PicMotionParams *motion = &p_Img->enc_picture->motion;
imgpel ***temp_img; // to temp store the Y data for 8x8 transform
int block, mode, i=0, j;
RD_PARAMS enc_mb;
int bmcost[5] = {INT_MAX};
double rd_cost = 0;
int cost = 0;
int min_cost = INT_MAX, cost_direct=0, have_direct=0;
int intra1 = 0;
int temp_cpb = 0;
Boolean best_transform_flag = FALSE;
int cost8x8_direct = 0;
short islice = (short) (currSlice->slice_type == I_SLICE);
short bslice = (short) (currSlice->slice_type == B_SLICE);
short pslice = (short) ((currSlice->slice_type == P_SLICE) || (currSlice->slice_type == SP_SLICE));
short intra = (short) (islice || (pslice && currMB->mb_y == p_Img->mb_y_upd && p_Img->mb_y_upd!=p_Img->mb_y_intra));
int lambda_mf[3];
Block8x8Info *b8x8info = p_Img->b8x8info;
//int mb_available[3] = { 1, 1, 1};
char **ipredmodes = p_Img->ipredmode;
short *allmvs = (currSlice->slice_type == I_SLICE) ? NULL: currSlice->all_mv[0][0][0][0][0];
int ****i4p; //for non-RD-opt. mode
imgpel **mb_pred = currSlice->mb_pred[0];
Boolean tmp_8x8_flag, tmp_no_mbpart;
BestMode md_best;
Info8x8 best;
init_md_best(&md_best);
// Init best (need to create simple function)
best.pdir = 0;
best.bipred = 0;
best.ref[LIST_0] = 0;
best.ref[LIST_1] = -1;
get_mem3Dpel(&temp_img, 3, MB_BLOCK_SIZE, MB_BLOCK_SIZE);
intra |= RandomIntra (p_Img, currMB->mbAddrX); // Forced Pseudo-Random Intra
//===== Setup Macroblock encoding parameters =====
init_enc_mb_params(currMB, &enc_mb, intra);
if (p_Inp->AdaptiveRounding)
{
reset_adaptive_rounding(p_Img);
}
if (currSlice->MbaffFrameFlag)
{
reset_mb_nz_coeff(p_Img, currMB->mbAddrX);
}
//===== S T O R E C O D I N G S T A T E =====
//---------------------------------------------------
currSlice->store_coding_state (currMB, currSlice->p_RDO->cs_cm);
if (!intra)
{
//===== set direct motion vectors =====
currMB->best_mode = 10; // let us set best_mode to an intra mode to avoid possible bug with RDOQ
if (bslice && enc_mb.valid[0])
{
currSlice->Get_Direct_Motion_Vectors (currMB);
}
if (p_Inp->CtxAdptLagrangeMult == 1)
{
get_initial_mb16x16_cost(currMB);
}
//===== MOTION ESTIMATION FOR 16x16, 16x8, 8x16 BLOCKS =====
for (mode = 1; mode < 4; mode++)
{
best.bipred = 0;
best.mode = (char) mode;
b8x8info->best[mode][0].bipred = 0;
if (enc_mb.valid[mode])
{
for (cost=0, block=0; block<(mode==1?1:2); block++)
{
update_lambda_costs(currMB, &enc_mb, lambda_mf);
PartitionMotionSearch (currMB, mode, block, lambda_mf);
//--- set 4x4 block indizes (for getting MV) ---
j = (block==1 && mode==2 ? 2 : 0);
i = (block==1 && mode==3 ? 2 : 0);
//--- get cost and reference frame for List 0 prediction ---
bmcost[LIST_0] = INT_MAX;
list_prediction_cost(currMB, LIST_0, block, mode, &enc_mb, bmcost, best.ref);
if (bslice)
{
//--- get cost and reference frame for List 1 prediction ---
bmcost[LIST_1] = INT_MAX;
list_prediction_cost(currMB, LIST_1, block, mode, &enc_mb, bmcost, best.ref);
// Compute bipredictive cost between best list 0 and best list 1 references
list_prediction_cost(currMB, BI_PRED, block, mode, &enc_mb, bmcost, best.ref);
// currently Bi predictive ME is only supported for modes 1, 2, 3 and ref 0
if (is_bipred_enabled(p_Inp, mode))
{
list_prediction_cost(currMB, BI_PRED_L0, block, mode, &enc_mb, bmcost, 0);
list_prediction_cost(currMB, BI_PRED_L1, block, mode, &enc_mb, bmcost, 0);
}
else
{
bmcost[BI_PRED_L0] = INT_MAX;
bmcost[BI_PRED_L1] = INT_MAX;
}
// Determine prediction list based on mode cost
determine_prediction_list(bmcost, &best, &cost);
}
else // if (bslice)
{
best.pdir = 0;
cost += bmcost[LIST_0];
}
assign_enc_picture_params(currMB, mode, &best, 2 * block);
//----- set reference frame and direction parameters -----
set_block8x8_info(b8x8info, mode, block, &best);
//--- set reference frames and motion vectors ---
if (mode>1 && block==0)
currSlice->set_ref_and_motion_vectors (currMB, motion, &best, block);
} // for (block=0; block<(mode==1?1:2); block++)
currMB->luma_transform_size_8x8_flag = FALSE;
if (p_Inp->Transform8x8Mode) //for inter rd-off, set 8x8 to do 8x8 transform
{
SetModesAndRefframeForBlocks(currMB, (short) mode);
currMB->luma_transform_size_8x8_flag = (byte) TransformDecision(currMB, -1, &cost);
}
if (cost < min_cost)
{
currMB->best_mode = (short) mode;
min_cost = cost;
best_transform_flag = currMB->luma_transform_size_8x8_flag;
if (p_Inp->CtxAdptLagrangeMult == 1)
{
adjust_mb16x16_cost(currMB, cost);
}
}
} // if (enc_mb.valid[mode])
} // for (mode=1; mode<4; mode++)
if (enc_mb.valid[P8x8])
{
//===== store coding state of macroblock =====
currSlice->store_coding_state (currMB, currSlice->p_RDO->cs_mb);
memset( currSlice->cofAC[0][0][0], 0, 2080 * sizeof(int)); // 4 * 4 * 2 * 65
currMB->valid_8x8 = FALSE;
if (p_Inp->Transform8x8Mode)
{
ResetRD8x8Data(p_Img, p_RDO->tr8x8);
//===========================================================
// Check 8x8 partition with transform size 8x8
//===========================================================
//===== LOOP OVER 8x8 SUB-PARTITIONS (Motion Estimation & Mode Decision) =====
for (cost_direct = 0, block = 0; block < 4; block++)
{
submacroblock_mode_decision_low(currMB, &enc_mb, p_RDO->tr8x8, p_RDO->cofAC8x8ts[block],
&have_direct, block, &cost_direct, &cost, &cost8x8_direct, 1);
set_subblock8x8_info(b8x8info, P8x8, block, p_RDO->tr8x8);
}
currMB->luma_transform_size_8x8_flag = FALSE; //switch to 4x4 transform size
}// if (p_Inp->Transform8x8Mode)
if (p_Inp->Transform8x8Mode != 2)
{
ResetRD8x8Data(p_Img, p_RDO->tr4x4);
//=================================================================
// Check 8x8, 8x4, 4x8 and 4x4 partitions with transform size 4x4
//=================================================================
//===== LOOP OVER 8x8 SUB-PARTITIONS (Motion Estimation & Mode Decision) =====
for (cost_direct = 0, block=0; block<4; block++)
{
submacroblock_mode_decision_low(currMB, &enc_mb, p_RDO->tr4x4, p_RDO->coefAC8x8[block],
&have_direct, block, &cost_direct, &cost, &cost8x8_direct, 0);
set_subblock8x8_info(b8x8info, P8x8, block, p_RDO->tr4x4);
}
}// if (p_Inp->Transform8x8Mode != 2)
if (p_Inp->RCEnable)
rc_store_diff(currSlice->diffy, &p_Img->pCurImg[currMB->opix_y], currMB->pix_x, mb_pred);
//check cost for P8x8 for non-rdopt mode
if (((p_Inp->Transform8x8Mode < 2) && (p_RDO->tr4x4->mb_p8x8_cost < min_cost)) ||
((p_Inp->Transform8x8Mode > 0) && (p_RDO->tr8x8->mb_p8x8_cost < min_cost)))
{
currMB->best_mode = P8x8;
if (p_Inp->Transform8x8Mode == 2)
{
min_cost = p_RDO->tr8x8->mb_p8x8_cost;
currMB->luma_transform_size_8x8_flag = TRUE;
}
else if (p_Inp->Transform8x8Mode)
{
if (p_RDO->tr8x8->mb_p8x8_cost < p_RDO->tr4x4->mb_p8x8_cost)
{
min_cost = p_RDO->tr8x8->mb_p8x8_cost;
currMB->luma_transform_size_8x8_flag = TRUE;
}
else if(p_RDO->tr4x4->mb_p8x8_cost < p_RDO->tr8x8->mb_p8x8_cost)
{
min_cost = p_RDO->tr4x4->mb_p8x8_cost;
currMB->luma_transform_size_8x8_flag = FALSE;
}
else
{
if (GetBestTransformP8x8(currMB) == 0)
{
min_cost = p_RDO->tr4x4->mb_p8x8_cost;
currMB->luma_transform_size_8x8_flag = FALSE;
}
else
{
min_cost = p_RDO->tr8x8->mb_p8x8_cost;
currMB->luma_transform_size_8x8_flag = TRUE;
}
}
}
else
{
min_cost = p_RDO->tr4x4->mb_p8x8_cost;
currMB->luma_transform_size_8x8_flag = FALSE;
}
}// if ((p_RDO->tr4x4->mb_p8x8_cost < min_cost || p_RDO->tr8x8->mb_p8x8_cost < min_cost))
p_Img->giRDOpt_B8OnlyFlag = FALSE;
}
// Find a motion vector for the Skip mode
if(pslice)
FindSkipModeMotionVector (currMB);
}
else // if (!intra)
{
min_cost = INT_MAX;
}
//========= C H O O S E B E S T M A C R O B L O C K M O D E =========
//-------------------------------------------------------------------------
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag; //save 8x8_flag
tmp_no_mbpart = currMB->NoMbPartLessThan8x8Flag; //save no-part-less
if ((p_Img->yuv_format != YUV400) && (p_Img->yuv_format != YUV444))
// precompute all chroma intra prediction modes
intra_chroma_prediction(currMB, NULL, NULL, NULL);
if (enc_mb.valid[0] && bslice) // check DIRECT MODE
{
if(have_direct)
{
switch(p_Inp->Transform8x8Mode)
{
case 1: // Mixture of 8x8 & 4x4 transform
cost = ((cost8x8_direct < cost_direct) || !(enc_mb.valid[5] && enc_mb.valid[6] && enc_mb.valid[7]))
? cost8x8_direct : cost_direct;
break;
case 2: // 8x8 Transform only
cost = cost8x8_direct;
break;
default: // 4x4 Transform only
cost = cost_direct;
break;
}
}
else
{ //!have_direct
cost = GetDirectCostMB (currMB);
}
if (cost!=INT_MAX)
{
cost -= (int)floor(16 * enc_mb.lambda_md + 0.4999);
}
if (cost <= min_cost)
{
if(p_Img->active_sps->direct_8x8_inference_flag && p_Inp->Transform8x8Mode)
{
if(p_Inp->Transform8x8Mode==2)
currMB->luma_transform_size_8x8_flag = TRUE;
else
{
if(cost8x8_direct < cost_direct)
currMB->luma_transform_size_8x8_flag = TRUE;
else
currMB->luma_transform_size_8x8_flag = FALSE;
}
}
else
currMB->luma_transform_size_8x8_flag = FALSE;
//Rate control
if (p_Inp->RCEnable)
rc_store_diff(currSlice->diffy, &p_Img->pCurImg[currMB->opix_y], currMB->pix_x, mb_pred);
min_cost = cost;
currMB->best_mode = 0;
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag;
}
else
{
currMB->luma_transform_size_8x8_flag = (byte) tmp_8x8_flag; // restore if not best
currMB->NoMbPartLessThan8x8Flag = (byte) tmp_no_mbpart; // restore if not best
}
}
currMB->min_rdcost = (double) min_cost;
if (enc_mb.valid[I8MB]) // check INTRA8x8
{
currMB->luma_transform_size_8x8_flag = TRUE; // at this point cost will ALWAYS be less than min_cost
currMB->mb_type = currMB->ar_mode = I8MB;
temp_cpb = Mode_Decision_for_Intra8x8Macroblock (currMB, enc_mb.lambda_md, &rd_cost);
if (rd_cost <= currMB->min_rdcost) //HYU_NOTE. bug fix. 08/15/07
{
currMB->cbp = temp_cpb;
if (p_Img->P444_joined)
{
currSlice->curr_cbp[0] = currSlice->cmp_cbp[1];
currSlice->curr_cbp[1] = currSlice->cmp_cbp[2];
}
if(enc_mb.valid[I4MB])
{
//coeffs
if (p_Inp->Transform8x8Mode != 2)
{
i4p = p_RDO->cofAC;
p_RDO->cofAC = currSlice->cofAC;
currSlice->cofAC = i4p;
}
}
copy_image_data_16x16(temp_img[0], &p_Img->enc_picture->imgY[currMB->pix_y], 0, currMB->pix_x);
if (p_Img->P444_joined)
{
copy_image_data_16x16(temp_img[1], &p_Img->enc_picture->imgUV[0][currMB->pix_y], 0, currMB->pix_x);
copy_image_data_16x16(temp_img[2], &p_Img->enc_picture->imgUV[1][currMB->pix_y], 0, currMB->pix_x);
}
//Rate control
if (p_Inp->RCEnable)
rc_store_diff(currSlice->diffy, &p_Img->pCurImg[currMB->opix_y], currMB->pix_x, mb_pred);
currMB->min_rdcost = rd_cost;
currMB->best_mode = I8MB;
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag;
}
else
{
currMB->luma_transform_size_8x8_flag = (byte) tmp_8x8_flag; // restore if not best
if (p_Img->P444_joined)
{
currMB->cbp |= currSlice->curr_cbp[0];
currMB->cbp |= currSlice->curr_cbp[1];
currSlice->cmp_cbp[1] = currMB->cbp;
currSlice->cmp_cbp[2] = currMB->cbp;
}
}
}
if (enc_mb.valid[I4MB]) // check INTRA4x4
{
currMB->luma_transform_size_8x8_flag = FALSE;
currMB->mb_type = currMB->ar_mode = I4MB;
temp_cpb = Mode_Decision_for_Intra4x4Macroblock (currMB, enc_mb.lambda_md, &rd_cost);
if (rd_cost <= currMB->min_rdcost)
{
currMB->cbp = temp_cpb;
//Rate control
if (p_Inp->RCEnable)
rc_store_diff(currSlice->diffy, &p_Img->pCurImg[currMB->opix_y], currMB->pix_x, mb_pred);
currMB->min_rdcost = rd_cost;
currMB->best_mode = I4MB;
tmp_8x8_flag = currMB->luma_transform_size_8x8_flag;
}
else
{
currMB->luma_transform_size_8x8_flag = (byte) tmp_8x8_flag; // restore if not best
if (p_Img->P444_joined)
{
currMB->cbp |= currSlice->curr_cbp[0];
currMB->cbp |= currSlice->curr_cbp[1];
currSlice->cmp_cbp[1] = currMB->cbp;
currSlice->cmp_cbp[2] = currMB->cbp;
}
//coeffs
i4p = p_RDO->cofAC;
p_RDO->cofAC = currSlice->cofAC;
currSlice->cofAC=i4p;
}
}
if (enc_mb.valid[I16MB]) // check INTRA16x16
{
currMB->luma_transform_size_8x8_flag = FALSE;
intrapred_16x16 (currMB, PLANE_Y);
if (p_Img->P444_joined)
{
select_plane(p_Img, PLANE_U);
intrapred_16x16 (currMB, PLANE_U);
select_plane(p_Img, PLANE_V);
intrapred_16x16 (currMB, PLANE_V);
select_plane(p_Img, PLANE_Y);
}
rd_cost = currSlice->find_sad_16x16 (currMB);
if (rd_cost < currMB->min_rdcost)
{
//Rate control
if (p_Inp->RCEnable)
rc_store_diff(currSlice->diffy, &p_Img->pCurImg[currMB->opix_y], currMB->pix_x, currSlice->mpr_16x16[0][(short) currMB->i16mode]);
currMB->best_mode = I16MB;
currMB->min_rdcost = rd_cost;
currMB->cbp = currMB->trans_16x16 (currMB, PLANE_Y);
if (p_Img->P444_joined)
{
select_plane(p_Img, PLANE_U);
currSlice->cmp_cbp[1] = currMB->trans_16x16(currMB, PLANE_U);
select_plane(p_Img, PLANE_V);
currSlice->cmp_cbp[2] = currMB->trans_16x16(currMB, PLANE_V);
select_plane(p_Img, PLANE_Y);
currMB->cbp |= currSlice->cmp_cbp[1];
currMB->cbp |= currSlice->cmp_cbp[2];
currSlice->cmp_cbp[1] = currMB->cbp;
currSlice->cmp_cbp[2] = currMB->cbp;
}
}
else
{
currMB->luma_transform_size_8x8_flag = (byte) tmp_8x8_flag; // restore
currMB->NoMbPartLessThan8x8Flag = (byte) tmp_no_mbpart; // restore
}
}
intra1 = IS_INTRA(currMB);
//===== S E T F I N A L M A C R O B L O C K P A R A M E T E R S ======
//---------------------------------------------------------------------------
{
//===== set parameters for chosen mode =====
SetModesAndRefframeForBlocks (currMB, currMB->best_mode);
if (currMB->best_mode == P8x8)
{
if (currMB->luma_transform_size_8x8_flag && (p_RDO->tr8x8->cbp8x8 == 0) && p_Inp->Transform8x8Mode != 2)
currMB->luma_transform_size_8x8_flag = FALSE;
SetCoeffAndReconstruction8x8 (currMB);
memset(currMB->intra_pred_modes, DC_PRED, MB_BLOCK_PARTITIONS * sizeof(char));
for (j = currMB->block_y; j < currMB->block_y + BLOCK_MULTIPLE; j++)
memset(&ipredmodes[j][currMB->block_x], DC_PRED, BLOCK_MULTIPLE * sizeof(char));
}
else
{
//===== set parameters for chosen mode =====
if (currMB->best_mode == I8MB)
{
memcpy(currMB->intra_pred_modes,currMB->intra_pred_modes8x8, MB_BLOCK_PARTITIONS * sizeof(char));
for(j = currMB->block_y; j < currMB->block_y + BLOCK_MULTIPLE; j++)
memcpy(&p_Img->ipredmode[j][currMB->block_x],&p_Img->ipredmode8x8[j][currMB->block_x], BLOCK_MULTIPLE * sizeof(char));
//--- restore reconstruction for 8x8 transform ---
copy_image_data_16x16(&p_Img->enc_picture->imgY[currMB->pix_y], temp_img[0], currMB->pix_x, 0);
if (p_Img->P444_joined)
{
copy_image_data_16x16(&p_Img->enc_picture->imgUV[0][currMB->pix_y], temp_img[1], currMB->pix_x, 0);
copy_image_data_16x16(&p_Img->enc_picture->imgUV[1][currMB->pix_y], temp_img[2], currMB->pix_x, 0);
}
}
if ((currMB->best_mode!=I4MB)&&(currMB->best_mode != I8MB))
{
memset(currMB->intra_pred_modes,DC_PRED, MB_BLOCK_PARTITIONS * sizeof(char));
for(j = currMB->block_y; j < currMB->block_y + BLOCK_MULTIPLE; j++)
memset(&ipredmodes[j][currMB->block_x],DC_PRED, BLOCK_MULTIPLE * sizeof(char));
currMB->ar_mode = currMB->best_mode;
if (currMB->best_mode!=I16MB)
{
if((currMB->best_mode >= 1) && (currMB->best_mode <= 3))
currMB->luma_transform_size_8x8_flag = (byte) best_transform_flag;
if (currSlice->P444_joined)
{
luma_residual_coding_p444(currMB);
if((currMB->cbp==0 && currSlice->cmp_cbp[1] == 0 && currSlice->cmp_cbp[2] == 0) &&(currMB->best_mode == 0))
currMB->luma_transform_size_8x8_flag = FALSE;
}
else
{
luma_residual_coding(currMB);
if((currMB->cbp==0)&&(currMB->best_mode == 0))
currMB->luma_transform_size_8x8_flag = FALSE;
}
//Rate control
if (p_Inp->RCEnable)
rc_store_diff(currSlice->diffy, &p_Img->pCurImg[currMB->opix_y], currMB->pix_x, mb_pred);
}
}
}
//check luma cbp for transform size flag
if (((currMB->cbp&15) == 0) && currMB->mb_type != I4MB && currMB->mb_type != I8MB)
currMB->luma_transform_size_8x8_flag = FALSE;
// precompute all chroma intra prediction modes
if ((p_Img->yuv_format != YUV400) && (p_Img->yuv_format != YUV444))
intra_chroma_prediction(currMB, NULL, NULL, NULL);
currMB->i16offset = 0;
if ((p_Img->yuv_format != YUV400) && (p_Img->yuv_format != YUV444))
chroma_residual_coding (currMB);
if (currMB->best_mode == I16MB)
{
currMB->i16offset = I16Offset (currMB->cbp, currMB->i16mode);
}
currSlice->SetMotionVectorsMB (currMB, motion);
//===== check for SKIP mode =====
if(p_Img->P444_joined)
{
if ((pslice) && currMB->best_mode == 1 && currMB->cbp==0 && currSlice->cmp_cbp[1] == 0 && currSlice->cmp_cbp[2] == 0 &&
motion->ref_idx[LIST_0][currMB->block_y][currMB->block_x] == 0 &&
motion->mv [LIST_0][currMB->block_y][currMB->block_x][0] == allmvs[0] &&
motion->mv [LIST_0][currMB->block_y][currMB->block_x][1] == allmvs[1])
{
currMB->mb_type = currMB->b8x8[0].mode = currMB->b8x8[1].mode = currMB->b8x8[2].mode = currMB->b8x8[3].mode = 0;
currMB->luma_transform_size_8x8_flag = FALSE;
}
}
else if ((pslice) && currMB->best_mode == 1 && currMB->cbp==0 &&
motion->ref_idx[LIST_0][currMB->block_y][currMB->block_x] == 0 &&
motion->mv [LIST_0][currMB->block_y][currMB->block_x][0] == allmvs[0] &&
motion->mv [LIST_0][currMB->block_y][currMB->block_x][1] == allmvs[1])
{
currMB->mb_type = currMB->b8x8[0].mode = currMB->b8x8[1].mode = currMB->b8x8[2].mode = currMB->b8x8[3].mode = 0;
currMB->luma_transform_size_8x8_flag = FALSE;
}
if (currSlice->MbaffFrameFlag || (p_Inp->UseRDOQuant && currSlice->RDOQ_QP_Num > 1))
set_mbaff_parameters(currMB);
}
// Rate control
if(p_Inp->RCEnable && p_Inp->RCUpdateMode <= MAX_RC_MODE)
rc_store_mad(currMB);
//===== Decide if this MB will restrict the reference frames =====
if (p_Inp->RestrictRef)
update_refresh_map(currMB, intra, intra1);
/*update adaptive rounding offset p_Inp*/
if (p_Img->AdaptiveRounding)
{
update_offset_params(currMB, currMB->best_mode, currMB->luma_transform_size_8x8_flag);
}
free_mem3Dpel(temp_img);
}
/*!
*************************************************************************************
* \brief
* R-D Cost for an 8x8 Intra block
*************************************************************************************
*/
distblk rdcost_for_8x8_intra_blocks_444(Macroblock *currMB, int *nonzero, int b8, int ipmode, int lambda, distblk min_rdcost, int mostProbableMode)
{
VideoParameters *p_Vid = currMB->p_Vid;
Slice *currSlice = currMB->p_Slice;
distblk rdcost = 0;
int dummy = 0;
int rate;
distblk distortion = 0;
int block_x = (b8 & 0x01) << 3;
int block_y = (b8 >> 1) << 3;
int pic_pix_x = currMB->pix_x + block_x;
int pic_pix_y = currMB->pix_y + block_y;
int pic_opix_y = currMB->opix_y + block_y;
SyntaxElement se;
const int *partMap = assignSE2partition[currSlice->partition_mode];
DataPartition *dataPart;
if(currSlice->P444_joined == 0)
{
//===== perform forward transform, Q, IQ, inverse transform, Reconstruction =====
*nonzero = currMB->residual_transform_quant_luma_8x8 (currMB, PLANE_Y, b8, &dummy, 1);
//===== get distortion (SSD) of 8x8 block =====
distortion += compute_SSE8x8(&p_Vid->pCurImg[pic_opix_y], &p_Vid->enc_picture->imgY[pic_pix_y], pic_pix_x, pic_pix_x);
currMB->ipmode_DPCM = NO_INTRA_PMODE;
//===== RATE for INTRA PREDICTION MODE (SYMBOL MODE MUST BE SET TO CAVLC) =====
se.value1 = (mostProbableMode == ipmode) ? -1 : ipmode < mostProbableMode ? ipmode : ipmode-1;
//--- set position and type ---
se.context = b8;
se.type = SE_INTRAPREDMODE;
//--- choose data partition ---
if (currSlice->slice_type != B_SLICE)
dataPart = &(currSlice->partArr[partMap[SE_INTRAPREDMODE]]);
else
dataPart = &(currSlice->partArr[partMap[SE_BFRAME]]);
//--- encode and update rate ---
currSlice->writeIntraPredMode (&se, dataPart);
rate = se.len;
//===== RATE for LUMINANCE COEFFICIENTS =====
if (currSlice->symbol_mode == CAVLC)
{
rate += currSlice->writeCoeff4x4_CAVLC (currMB, LUMA, b8, 0, 0);
rate += currSlice->writeCoeff4x4_CAVLC (currMB, LUMA, b8, 1, 0);
rate += currSlice->writeCoeff4x4_CAVLC (currMB, LUMA, b8, 2, 0);
rate += currSlice->writeCoeff4x4_CAVLC (currMB, LUMA, b8, 3, 0);
}
else
{
rate += writeCoeff8x8_CABAC (currMB, PLANE_Y, b8, 1);
}
}
else
{
ColorPlane k;
//===== perform forward transform, Q, IQ, inverse transform, Reconstruction =====
*nonzero = currMB->residual_transform_quant_luma_8x8 (currMB, PLANE_Y, b8, &dummy, 1);
//===== get distortion (SSD) of 8x8 block =====
distortion += compute_SSE8x8(&p_Vid->pCurImg[pic_opix_y], &p_Vid->enc_picture->imgY[pic_pix_y], pic_pix_x, pic_pix_x);
for (k = PLANE_U; k <= PLANE_V; k++)
{
select_plane(p_Vid, k);
currMB->c_nzCbCr[k ]= currMB->residual_transform_quant_luma_8x8(currMB, k, b8, &dummy,1);
distortion += compute_SSE8x8(&p_Vid->pImgOrg[k][pic_opix_y], &p_Vid->enc_picture->p_curr_img[pic_pix_y], pic_pix_x, pic_pix_x);
}
currMB->ipmode_DPCM = NO_INTRA_PMODE;
select_plane(p_Vid, PLANE_Y);
//===== RATE for INTRA PREDICTION MODE (SYMBOL MODE MUST BE SET TO CAVLC) =====
se.value1 = (mostProbableMode == ipmode) ? -1 : ipmode < mostProbableMode ? ipmode : ipmode-1;
//--- set position and type ---
se.context = b8;
se.type = SE_INTRAPREDMODE;
//--- choose data partition ---
if (currSlice->slice_type != B_SLICE)
dataPart = &(currSlice->partArr[partMap[SE_INTRAPREDMODE]]);
else
dataPart = &(currSlice->partArr[partMap[SE_BFRAME]]);
//--- encode and update rate ---
currSlice->writeIntraPredMode (&se, dataPart);
rate = se.len;
//===== RATE for LUMINANCE COEFFICIENTS =====
if (currSlice->symbol_mode == CAVLC)
{
int b4;
for(b4=0; b4<4; b4++)
{
rate += currSlice->writeCoeff4x4_CAVLC (currMB, LUMA, b8, b4, 0);
rate += currSlice->writeCoeff4x4_CAVLC (currMB, CB, b8, b4, 0);
rate += currSlice->writeCoeff4x4_CAVLC (currMB, CR, b8, b4, 0);
}
}
else
{
rate += writeCoeff8x8_CABAC (currMB, PLANE_Y, b8, 1);
rate += writeCoeff8x8_CABAC (currMB, PLANE_U, b8, 1);
rate += writeCoeff8x8_CABAC (currMB, PLANE_V, b8, 1);
}
}
rdcost = distortion + weight_cost(lambda, rate);
currSlice->reset_coding_state (currMB, currSlice->p_RDO->cs_cm);
return rdcost;
}
