void nova(int ix, int iy) {
static double course[] =
{0.0, 10.5, 12.0, 1.5, 9.0, 0.0, 3.0, 7.5, 6.0, 4.5};
int bot, top, top2, burst, hits[11][3], kount, icx, icy, mm, nn, j;
int iquad, iquad1, i, ll, newcx, newcy, ii, jj;
if (Rand() < 0.05) {
/* Wow! We've supernova'ed */
snova(ix, iy);
return;
}
/* handle initial nova */
quad[ix][iy] = IHDOT;
crmena(1, IHSTAR, 2, ix, iy);
prout(" novas.");
d.galaxy[quadx][quady] -= 1;
d.starkl++;
/* Set up stack to recursively trigger adjacent stars */
bot = top = top2 = 1;
kount = 0;
icx = icy = 0;
hits[1][1] = ix;
hits[1][2] = iy;
while (1) {
for (mm = bot; mm <= top; mm++)
for (nn = 1; nn <= 3; nn++) /* nn,j represents coordinates around current */
for (j = 1; j <= 3; j++) {
if (j==2 && nn== 2) continue;
ii = hits[mm][1]+nn-2;
jj = hits[mm][2]+j-2;
if (ii < 1 || ii > 10 || jj < 1 || jj > 10) continue;
iquad = quad[ii][jj];
switch (iquad) {
// case IHDOT: /* Empty space ends reaction
// case IHQUEST:
// case IHBLANK:
// case IHT:
// case IHWEB:
default:
break;
case IHSTAR: /* Affect another star */
if (Rand() < 0.05) {
/* This star supernovas */
snova(ii,jj);
return;
}
top2++;
hits[top2][1]=ii;
hits[top2][2]=jj;
d.galaxy[quadx][quady] -= 1;
d.starkl++;
crmena(1, IHSTAR, 2, ii, jj);
prout(" novas.");
quad[ii][jj] = IHDOT;
break;
case IHP: /* Destroy planet */
d.newstuf[quadx][quady] -= 1;
d.nplankl++;
crmena(1, IHP, 2, ii, jj);
prout(" destroyed.");
d.plnets[iplnet] = nulplanet;
iplnet = plnetx = plnety = 0;
if (landed == 1) {
finish(FPNOVA);
return;
}
quad[ii][jj] = IHDOT;
break;
case IHB: /* Destroy base */
d.galaxy[quadx][quady] -= 10;
for (i = 1; i <= d.rembase; i++)
if (d.baseqx[i]==quadx && d.baseqy[i]==quady) break;
d.baseqx[i] = d.baseqx[d.rembase];
d.baseqy[i] = d.baseqy[d.rembase];
d.rembase--;
basex = basey = 0;
d.basekl++;
newcnd();
crmena(1, IHB, 2, ii, jj);
prout(" destroyed.");
quad[ii][jj] = IHDOT;
break;
case IHE: /* Buffet ship */
case IHF:
prout("***Starship buffeted by nova.");
if (shldup) {
if (shield >= 2000.0) shield -= 2000.0;
else {
double diff = 2000.0 - shield;
energy -= diff;
shield = 0.0;
shldup = 0;
prout("***Shields knocked out.");
damage[DSHIELD] += 0.005*damfac*Rand()*diff;
}
}
else energy -= 2000.0;
if (energy <= 0) {
finish(FNOVA);
return;
}
/* add in course nova contributes to kicking starship*/
icx += sectx-hits[mm][1];
icy += secty-hits[mm][2];
kount++;
break;
case IHK: /* kill klingon */
deadkl(ii,jj,iquad, ii, jj);
break;
case IHC: /* Damage/destroy big enemies */
case IHS:
case IHR:
for (ll = 1; ll <= nenhere; ll++)
if (kx[ll]==ii && ky[ll]==jj) break;
kpower[ll] -= 800.0; /* If firepower is lost, die */
if (kpower[ll] <= 0.0) {
deadkl(ii, jj, iquad, ii, jj);
break;
}
newcx = ii + ii - hits[mm][1];
newcy = jj + jj - hits[mm][2];
crmena(1, iquad, 2, ii, jj);
proutn(" damaged");
if (newcx<1 || newcx>10 || newcy<1 || newcy>10) {
/* can't leave quadrant */
skip(1);
break;
}
iquad1 = quad[newcx][newcy];
if (iquad1 == IHBLANK) {
proutn(", blasted into ");
crmena(0, IHBLANK, 2, newcx, newcy);
skip(1);
deadkl(ii, jj, iquad, newcx, newcy);
break;
}
if (iquad1 != IHDOT) {
/* can't move into something else */
skip(1);
break;
}
proutn(", buffeted to");
cramlc(2, newcx, newcy);
quad[ii][jj] = IHDOT;
quad[newcx][newcy] = iquad;
kx[ll] = newcx;
ky[ll] = newcy;
kavgd[ll] = sqrt(square(sectx-newcx)+square(secty-newcy));
kdist[ll] = kavgd[ll];
skip(1);
break;
}
}
if (top == top2) break;
bot = top + 1;
top = top2;
}
if (kount==0) return;
/* Starship affected by nova -- kick it away. */
dist = kount*0.1;
if (icx) icx = (icx < 0 ? -1 : 1);
if (icy) icy = (icy < 0 ? -1 : 1);
direc = course[3*(icx+1)+icy+2];
if (direc == 0.0) dist = 0.0;
if (dist == 0.0) return;
Time = 10.0*dist/16.0;
skip(1);
prout("Force of nova displaces starship.");
iattak=2; /* Eliminates recursion problem */
lmove();
Time = 10.0*dist/16.0;
return;
}
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) {
manage_heater();
manage_inactivity(1);
LCD_STATUS;
}
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
if(degHotend(active_extruder)<EXTRUDE_MINTEMP && !allow_cold_extrude)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) { return; };
block->fan_speed = FanSpeed;
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_AXIS); }
if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_AXIS); }
if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_AXIS); }
if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); }
block->active_extruder = extruder;
//enable active axes
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
// Enable all
if(block->steps_e != 0) { enable_e0();enable_e1();enable_e2(); }
if (block->steps_e == 0) {
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else {
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
if ( block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0 ) {
block->millimeters = abs(delta_mm[E_AXIS]);
} else {
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) {
if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
if(abs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
if((direction_change & (1<<X_AXIS)) == 0) {
x_segment_time[0] += segment_time;
}
else {
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0) {
y_segment_time[0] += segment_time;
}
else {
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME) speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif
// Correct the speed
if( speed_factor < 1.0) {
for(unsigned char i=0; i < 4; i++) {
current_speed[i] *= speed_factor;
}
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / steps_per_mm;
block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2;
if(abs(current_speed[Z_AXIS]) > max_z_jerk/2)
vmax_junction = max_z_jerk/2;
vmax_junction = min(vmax_junction, block->nominal_speed);
if(abs(current_speed[E_AXIS]) > max_e_jerk/2)
vmax_junction = min(vmax_junction, max_e_jerk/2);
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
if((abs(previous_speed[X_AXIS]) > 0.0001) || (abs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
}
if (jerk > max_xy_jerk) {
vmax_junction *= (max_xy_jerk/jerk);
}
if(abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
vmax_junction *= (max_z_jerk/abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]));
}
if(abs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
vmax_junction *= (max_e_jerk/abs(current_speed[E_AXIS] - previous_speed[E_AXIS]));
}
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
MINIMUM_PLANNER_SPEED/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
st_wake_up();
}
/**
** Mark the sight of unit. (Explore and make visible.)
**
** @param player player to mark the sight for (not unit owner)
** @param pos location to mark
** @param w width to mark, in square
** @param h height to mark, in square
** @param range Radius to mark.
** @param marker Function to mark or unmark sight
*/
void MapSight(const CPlayer &player, const Vec2i &pos, int w, int h, int range, MapMarkerFunc *marker)
{
// Units under construction have no sight range.
if (!range) {
return;
}
// Up hemi-cyle
const int miny = std::max(-range, 0 - pos.y);
for (int offsety = miny; offsety != 0; ++offsety) {
const int offsetx = isqrt(square(range + 1) - square(-offsety) - 1);
const int minx = std::max(0, pos.x - offsetx);
const int maxx = std::min(Map.Info.MapWidth, pos.x + w + offsetx);
Vec2i mpos(minx, pos.y + offsety);
#ifdef MARKER_ON_INDEX
const unsigned int index = mpos.y * Map.Info.MapWidth;
#endif
for (mpos.x = minx; mpos.x < maxx; ++mpos.x) {
#ifdef MARKER_ON_INDEX
marker(player, mpos.x + index);
#else
marker(player, mpos);
#endif
}
}
for (int offsety = 0; offsety < h; ++offsety) {
const int minx = std::max(0, pos.x - range);
const int maxx = std::min(Map.Info.MapWidth, pos.x + w + range);
Vec2i mpos(minx, pos.y + offsety);
#ifdef MARKER_ON_INDEX
const unsigned int index = mpos.y * Map.Info.MapWidth;
#endif
for (mpos.x = minx; mpos.x < maxx; ++mpos.x) {
#ifdef MARKER_ON_INDEX
marker(player, mpos.x + index);
#else
marker(player, mpos);
#endif
}
}
// bottom hemi-cycle
const int maxy = std::min(range, Map.Info.MapHeight - pos.y - h);
for (int offsety = 0; offsety < maxy; ++offsety) {
const int offsetx = isqrt(square(range + 1) - square(offsety + 1) - 1);
const int minx = std::max(0, pos.x - offsetx);
const int maxx = std::min(Map.Info.MapWidth, pos.x + w + offsetx);
Vec2i mpos(minx, pos.y + h + offsety);
#ifdef MARKER_ON_INDEX
const unsigned int index = mpos.y * Map.Info.MapWidth;
#endif
for (mpos.x = minx; mpos.x < maxx; ++mpos.x) {
#ifdef MARKER_ON_INDEX
marker(player, mpos.x + index);
#else
marker(player, mpos);
#endif
}
}
}
void ARX_NPC_TryToCutSomething(Entity * target, const Vec3f * pos)
{
//return;
if(!target || !(target->ioflags & IO_NPC))
return;
if(target->gameFlags & GFLAG_NOGORE)
return;
float mindistSqr = std::numeric_limits<float>::max();
ObjSelection numsel = ObjSelection();
long goretex = -1;
for(size_t i = 0; i < target->obj->texturecontainer.size(); i++) {
if(target->obj->texturecontainer[i]
&& boost::contains(target->obj->texturecontainer[i]->m_texName.string(), "gore")
) {
goretex = i;
break;
}
}
for(size_t i = 0; i < target->obj->selections.size(); i++) {
ObjSelection sel = ObjSelection(i);
if(target->obj->selections[i].selected.size() > 0
&& boost::contains(target->obj->selections[i].name, "cut_")
) {
DismembermentFlag fll = GetCutFlag(target->obj->selections[i].name);
if(IsAlreadyCut(target, fll))
continue;
long out = 0;
for(size_t ll = 0; ll < target->obj->facelist.size(); ll++) {
EERIE_FACE & face = target->obj->facelist[ll];
if(face.texid != goretex) {
if( IsInSelection(target->obj, face.vid[0], sel)
|| IsInSelection(target->obj, face.vid[1], sel)
|| IsInSelection(target->obj, face.vid[2], sel)
) {
if(face.facetype & POLY_HIDE) {
out++;
}
}
}
}
if(out < 3) {
float dist = arx::distance2(*pos, target->obj->vertexlist3[target->obj->selections[i].selected[0]].v);
if(dist < mindistSqr) {
mindistSqr = dist;
numsel = sel;
}
}
}
}
if(numsel == ObjSelection())
return; // Nothing to cut...
bool hid = false;
if(mindistSqr < square(60)) { // can only cut a close part...
DismembermentFlag fl = GetCutFlag( target->obj->selections[numsel.handleData()].name );
if(fl && !(target->_npcdata->cuts & fl)) {
target->_npcdata->cuts |= fl;
hid = ARX_NPC_ApplyCuts(target);
}
}
if(hid) {
ARX_SOUND_PlayCinematic("flesh_critical", false); // TODO why play cinmeatic sound?
ARX_NPC_SpawnMember(target, numsel);
}
}
// calculate fft and then get the power
arma::mat powerFFT(arma::mat mat, int nfft)
{
arma::cx_mat cmat = fft(mat, nfft);
arma::mat result = square(real(cmat)) + square(imag(cmat));
return result;
}
int main(void)
{
/* Create an object that decodes the input video stream. */
VideoCapture cap(0); // open the default camera
if(!cap.isOpened()) // check if we succeeded
return -1;
Mat edges;
namedWindow("edges",1);
Mat frame;
cap >> frame; // get a new frame from camera
/*CvCapture *input_video = cvCaptureFromFile("C:\\Documents and Settings\\David Stavens\\Desktop\\223B-Demo\\optical_flow_input.avi");
if (input_video == NULL)
{
/* Either the video didn't exist OR it uses a codec OpenCV
* doesn't support.
*/
/*fprintf(stderr, "Error: Can't open video.\n");
return -1;
}*/
/* This is a hack. If we don't call this first then getting capture
* properties (below) won't work right. This is an OpenCV bug. We
* ignore the return value here. But it's actually a video frame.
*/
cvQueryFrame( frame );
/* Read the video's frame size out of the AVI. */
CvSize frame_size;
frame_size.height =
(int) cvGetCaptureProperty( input_video, CV_CAP_PROP_FRAME_HEIGHT );
frame_size.width =
(int) cvGetCaptureProperty( input_video, CV_CAP_PROP_FRAME_WIDTH );
/* Determine the number of frames in the AVI. */
long number_of_frames;
/* Go to the end of the AVI (ie: the fraction is "1") */
cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_AVI_RATIO, 1. );
/* Now that we're at the end, read the AVI position in frames */
number_of_frames = (int) cvGetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES );
/* Return to the beginning */
cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES, 0. );
/* Create three windows called "Frame N", "Frame N+1", and "Optical Flow"
* for visualizing the output. Have those windows automatically change their
* size to match the output.
*/
cvNamedWindow("Optical Flow", CV_WINDOW_AUTOSIZE);
long current_frame = 0;
while(true)
{
static IplImage *frame = NULL, *frame1 = NULL, *frame1_1C = NULL, *frame2_1C =
NULL, *eig_image = NULL, *temp_image = NULL, *pyramid1 = NULL, *pyramid2 = NULL;
/* Go to the frame we want. Important if multiple frames are queried in
* the loop which they of course are for optical flow. Note that the very
* first call to this is actually not needed. (Because the correct position
* is set outsite the for() loop.)
*/
cvSetCaptureProperty( input_video, CV_CAP_PROP_POS_FRAMES, current_frame );
/* Get the next frame of the video.
* IMPORTANT! cvQueryFrame() always returns a pointer to the _same_
* memory location. So successive calls:
* frame1 = cvQueryFrame();
* frame2 = cvQueryFrame();
* frame3 = cvQueryFrame();
* will result in (frame1 == frame2 && frame2 == frame3) being true.
* The solution is to make a copy of the cvQueryFrame() output.
*/
frame = cvQueryFrame( input_video );
if (frame == NULL)
{
/* Why did we get a NULL frame? We shouldn't be at the end. */
fprintf(stderr, "Error: Hmm. The end came sooner than we thought.\n");
return -1;
}
/* Allocate another image if not already allocated.
* Image has ONE challenge of color (ie: monochrome) with 8-bit "color" depth.
* This is the image format OpenCV algorithms actually operate on (mostly).
*/
allocateOnDemand( &frame1_1C, frame_size, IPL_DEPTH_8U, 1 );
/* Convert whatever the AVI image format is into OpenCV's preferred format.
* AND flip the image vertically. Flip is a shameless hack. OpenCV reads
* in AVIs upside-down by default. (No comment :-))
*/
cvConvertImage(frame, frame1_1C, CV_CVTIMG_FLIP);
/* We'll make a full color backup of this frame so that we can draw on it.
* (It's not the best idea to draw on the static memory space of cvQueryFrame().)
*/
allocateOnDemand( &frame1, frame_size, IPL_DEPTH_8U, 3 );
cvConvertImage(frame, frame1, CV_CVTIMG_FLIP);
/* Get the second frame of video. Sample principles as the first. */
frame = cvQueryFrame( input_video );
if (frame == NULL)
{
fprintf(stderr, "Error: Hmm. The end came sooner than we thought.\n");
return -1;
}
allocateOnDemand( &frame2_1C, frame_size, IPL_DEPTH_8U, 1 );
cvConvertImage(frame, frame2_1C, CV_CVTIMG_FLIP);
/* Shi and Tomasi Feature Tracking! */
/* Preparation: Allocate the necessary storage. */
allocateOnDemand( &eig_image, frame_size, IPL_DEPTH_32F, 1 );
allocateOnDemand( &temp_image, frame_size, IPL_DEPTH_32F, 1 );
/* Preparation: This array will contain the features found in frame 1. */
CvPoint2D32f frame1_features[400];
/* Preparation: BEFORE the function call this variable is the array size
* (or the maximum number of features to find). AFTER the function call
* this variable is the number of features actually found.
*/
int number_of_features;
/* I'm hardcoding this at 400. But you should make this a #define so that you can
* change the number of features you use for an accuracy/speed tradeoff analysis.
*/
number_of_features = 400;
/* Actually run the Shi and Tomasi algorithm!!
* "frame1_1C" is the input image.
* "eig_image" and "temp_image" are just workspace for the algorithm.
* The first ".01" specifies the minimum quality of the features (based on the
eigenvalues).
* The second ".01" specifies the minimum Euclidean distance between features.
* "NULL" means use the entire input image. You could point to a part of the
image.
* WHEN THE ALGORITHM RETURNS:
* "frame1_features" will contain the feature points.
* "number_of_features" will be set to a value <= 400 indicating the number of
feature points found.
*/
cvGoodFeaturesToTrack(frame1_1C, eig_image, temp_image, frame1_features, &
number_of_features, .01, .01, NULL);
/* Pyramidal Lucas Kanade Optical Flow! */
/* This array will contain the locations of the points from frame 1 in frame 2. */
CvPoint2D32f frame2_features[400];
/* The i-th element of this array will be non-zero if and only if the i-th feature
of
* frame 1 was found in frame 2.
*/
char optical_flow_found_feature[400];
/* The i-th element of this array is the error in the optical flow for the i-th
feature
* of frame1 as found in frame 2. If the i-th feature was not found (see the
array above)
* I think the i-th entry in this array is undefined.
*/
float optical_flow_feature_error[400];
/* This is the window size to use to avoid the aperture problem (see slide
"Optical Flow: Overview"). */
CvSize optical_flow_window = cvSize(3,3);
/* This termination criteria tells the algorithm to stop when it has either done
20 iterations or when
* epsilon is better than .3. You can play with these parameters for speed vs.
accuracy but these values
* work pretty well in many situations.
*/
CvTermCriteria optical_flow_termination_criteria
= cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );
/* This is some workspace for the algorithm.
* (The algorithm actually carves the image into pyramids of different resolutions
.)
*/
allocateOnDemand( &pyramid1, frame_size, IPL_DEPTH_8U, 1 );
allocateOnDemand( &pyramid2, frame_size, IPL_DEPTH_8U, 1 );
/* Actually run Pyramidal Lucas Kanade Optical Flow!!
* "frame1_1C" is the first frame with the known features.
* "frame2_1C" is the second frame where we want to find the first frame's
features.
* "pyramid1" and "pyramid2" are workspace for the algorithm.
* "frame1_features" are the features from the first frame.
* "frame2_features" is the (outputted) locations of those features in the second
frame.
* "number_of_features" is the number of features in the frame1_features array.
* "optical_flow_window" is the size of the window to use to avoid the aperture
problem.
* "5" is the maximum number of pyramids to use. 0 would be just one level.
* "optical_flow_found_feature" is as described above (non-zero iff feature found
by the flow).
* "optical_flow_feature_error" is as described above (error in the flow for this
feature).
* "optical_flow_termination_criteria" is as described above (how long the
algorithm should look).
* "0" means disable enhancements. (For example, the second aray isn't pre-
initialized with guesses.)
*/
cvCalcOpticalFlowPyrLK(frame1_1C, frame2_1C, pyramid1, pyramid2, frame1_features,
frame2_features, number_of_features, optical_flow_window, 5,
optical_flow_found_feature, optical_flow_feature_error,
optical_flow_termination_criteria, 0 );
/* For fun (and debugging :)), let's draw the flow field. */
for(int i = 0; i < number_of_features; i++)
{
/* If Pyramidal Lucas Kanade didn't really find the feature, skip it. */
if ( optical_flow_found_feature[i] == 0 )
continue;
int line_thickness;
line_thickness = 1;
/* CV_RGB(red, green, blue) is the red, green, and blue components
* of the color you want, each out of 255.
*/
CvScalar line_color;
line_color = CV_RGB(255,0,0);
/* Let's make the flow field look nice with arrows. */
/* The arrows will be a bit too short for a nice visualization because of the
high framerate
* (ie: there's not much motion between the frames). So let's lengthen them
by a factor of 3.
*/
CvPoint p,q;
p.x = (int) frame1_features[i].x;
p.y = (int) frame1_features[i].y;
q.x = (int) frame2_features[i].x;
q.y = (int) frame2_features[i].y;
;
double angle;
angle = atan2( (double) p.y - q.y, (double) p.x - q.x );
double hypotenuse;
hypotenuse = sqrt( square(p.y - q.y) + square(p.x - q.x) )
/* Here we lengthen the arrow by a factor of three. */
q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
q.y = (int) (p.y - 3 * hypotenuse * sin(angle));
/* Now we draw the main line of the arrow. */
}
/* "frame1" is the frame to draw on.
* "p" is the point where the line begins.
* "q" is the point where the line stops.
* "CV_AA" means antialiased drawing.
* "0" means no fractional bits in the center cooridinate or radius.
*/
cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
/* Now draw the tips of the arrow. I do some scaling so that the
* tips look proportional to the main line of the arrow.
*/
p.x = (int) (q.x + 9 * cos(angle + pi / 4));
p.y = (int) (q.y + 9 * sin(angle + pi / 4));
cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
p.x = (int) (q.x + 9 * cos(angle - pi / 4));
p.y = (int) (q.y + 9 * sin(angle - pi / 4));
cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
}
/* Now display the image we drew on. Recall that "Optical Flow" is the name of
* the window we created above.
*/
cvShowImage("Optical Flow", frame1);
/* And wait for the user to press a key (so the user has time to look at the
image).
* If the argument is 0 then it waits forever otherwise it waits that number of
milliseconds.
* The return value is the key the user pressed.
*/
int key_pressed;
key_pressed = cvWaitKey(0);
}
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
int current_temp;
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) {
manage_inactivity(1);
#if (MINIMUM_FAN_START_SPEED > 0)
manage_fan_start_speed();
#endif
}
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
current_temp = analog2temp( current_raw );
#if PREVENT_DANGEROUS_EXTRUDE > 0
if(target[E_AXIS]!=position[E_AXIS]) {
if(current_temp < EXTRUDE_MINTEMP && prevent_cold_extrude) {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
serial_send(TXT_COLD_EXTRUSION_PREVENTED_CRLF);
}
#if PREVENT_LENGTHY_EXTRUDE > 0
if(labs(target[E_AXIS]-position[E_AXIS]) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
serial_send(TXT_LONG_EXTRUSION_PREVENTED_CRLF);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = 0;
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e = (long)(block->steps_e * extrudemultiply / 100.0);
block->step_event_count = MAX(block->steps_x, MAX(block->steps_y, MAX(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= DROP_SEGMENTS) return;
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) block->direction_bits |= (1<<X_AXIS);
if (target[Y_AXIS] < position[Y_AXIS]) block->direction_bits |= (1<<Y_AXIS);
if (target[Z_AXIS] < position[Z_AXIS]) block->direction_bits |= (1<<Z_AXIS);
if (target[E_AXIS] < position[E_AXIS]) {
block->direction_bits |= (1<<E_AXIS);
//High Feedrate for retract
max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
retract_feedrate_aktiv = 1;
}
else {
if(retract_feedrate_aktiv) {
if(block->steps_e > 0) retract_feedrate_aktiv = 0;
}
else max_E_feedrate_calc = max_feedrate[E_AXIS];
}
#ifdef DELAY_ENABLE
if(block->steps_x != 0) {
enable_x();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_y != 0) {
enable_y();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_z != 0) {
enable_z();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_e != 0) {
enable_e();
delayMicroseconds(DELAY_ENABLE);
}
#else
//enable active axes
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
if(block->steps_z != 0) enable_z();
if(block->steps_e != 0) enable_e();
#endif
if (block->steps_e == 0) {
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else {
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
// slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
int moves_queued=(block_buffer_head-block_buffer_tail + block_buffer_size) & block_buffer_mask;
#ifdef SLOWDOWN
if(moves_queued < (block_buffer_size * 0.5) && moves_queued > MIN_MOVES_QUEUED_FOR_SLOWDOWN)
feed_rate = feed_rate*moves_queued / (float)(block_buffer_size * 0.5);
#endif
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(float)(axis_steps_per_unit[X_AXIS]);
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(float)(axis_steps_per_unit[Y_AXIS]);
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(float)(axis_steps_per_unit[Z_AXIS]);
//delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/(float)(axis_steps_per_unit[E_AXIS]))*extrudemultiply/100.0;
if ( block->steps_x <= DROP_SEGMENTS && block->steps_y <= DROP_SEGMENTS && block->steps_z <= DROP_SEGMENTS )
block->millimeters = fabs(delta_mm[E_AXIS]);
else
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
float inverse_millimeters = 1.0/(float)(block->millimeters); // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
/*
#ifdef SLOWDOWN
// segment time im micro seconds
long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued>0) && (moves_queued < (BLOCK_BUFFER_SIZE - 4))) {
if (segment_time<minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued);
}
}
else {
if (segment_time<minsegmenttime) segment_time=minsegmenttime;
}
#endif
// END OF SLOW DOWN SECTION
*/
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 3; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = fmin(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
if(fabs(current_speed[E_AXIS]) > max_E_feedrate_calc)
speed_factor = fmin(speed_factor, max_E_feedrate_calc / fabs(current_speed[E_AXIS]));
// Correct the speed
if( speed_factor < 1.0) {
for(unsigned char i=0; i < 4; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/(float)(block->millimeters);
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
else {
block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / (float)(steps_per_mm);
block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(float)(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2.0;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.0) vmax_junction = fmin(vmax_junction, max_z_jerk/2.0);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.0) vmax_junction = fmin(vmax_junction, max_e_jerk/2.0);
if(G92_reset_previous_speed == 1) {
vmax_junction = 0.1;
G92_reset_previous_speed = 0;
}
vmax_junction = fmin(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = (max_xy_jerk/(float)(jerk));
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk)
vmax_junction_factor= fmin(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk)
vmax_junction_factor = fmin(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
vmax_junction = fmin(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = fmin(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) block->nominal_length_flag = 1;
else block->nominal_length_flag = 0;
block->recalculate_flag = 1; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
calculate_trapezoid_for_block(block, block->entry_speed/(float)(block->nominal_speed),
safe_speed/(float)(block->nominal_speed));
// Move buffer head
CRITICAL_SECTION_START;
block_buffer_head = next_buffer_head;
CRITICAL_SECTION_END;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
#ifdef AUTOTEMP
getHighESpeed();
#endif
st_wake_up();
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/* Declare variables. */
double *obj,objective,*dX, *X, *dist,weight,curdist, *ind1,*ind2,*v1,*v2,*vo, *dummy;
double pev=1.0,mev=1.0,*ev,*av,*bv;
int m,p,n,inds;
int oldi1=-1,i1,i2,j,i,r,c;
int ione=1;
char *chu="U";
char *chl="L";
char *chn2="T";
char *chn="N";
double minusone=-1.0,one=1.0, zero=0.0;
/* Check for proper number of input and output arguments. */
if (nrhs!=4) mexErrMsgTxt("Exactly one or two input arguments required.");
if (nlhs > 2) {
mexErrMsgTxt("Too many output arguments.");
}
/* Check data type of input argument. */
if (!(mxIsDouble(prhs[0]))) {
mexErrMsgTxt("Input array must be of type double.");
}
n = mxGetN(Xp);
m = mxGetM(Xp);
c = mxGetNumberOfElements(distp);
/* Get the data. */
X = mxGetPr(Xp);
ind1 = mxGetPr(ip);
ind2 = mxGetPr(jp);
dist = mxGetPr(distp);
/* Create output matrix */
plhs[0]=mxCreateDoubleMatrix(m,n,mxREAL);
dX=mxGetPr(plhs[0]);
plhs[1]=mxCreateDoubleMatrix(1,1,mxREAL);
obj=mxGetPr(plhs[1]);
/* create space for dummy vectors */
dummy=malloc(m*sizeof(double));
weight=0;
oldi1=-1;
v1=&X[0];v2=&X[0];vo=&dX[0];
objective=0;
for(i=0;i<c;i++){
i1=(int) ind1[i]-1;
i2=(int) ind2[i]-1;
v2 =&X[(int) (i2*m)];
if(i1!=oldi1){
for(j=0;j<m;j++) {vo[j]=weight*v1[j]-dummy[j];dummy[j]=0;}
weight=0;
v1 =&X[(int) (i1*m)];
vo=&dX[(int) (i1*m)];
oldi1=i1;
};
curdist=0;
for(j=0;j<m;j++) curdist=curdist+square(v1[j]-v2[j]);
/* curdist=(curdist-dist[i])/dist[i];objective+=square(curdist);*/
curdist=(curdist-dist[i]);objective+=square(curdist);
for(j=0;j<m;j++) dummy[j]+=curdist*v2[j];
weight=weight+curdist;
}
for(j=0;j<m;j++) {vo[j]=weight*v1[j]-dummy[j];}
obj[0]=objective;
free(dummy);
}
static double
crosshair_sq_dist (CrosshairType *crosshair, Coord x, Coord y)
{
return square (x - crosshair->X) + square (y - crosshair->Y);
}
void Cterrain::getBlocks(int jmin,int jmax,int imin,int imax,int curDepth)
{
//检查当前节点是否与视截体相交--abc
//求节点p所表示区域的保守包围盒--abc
// 上面--abc
// p[0]--p[3]
// | |
// p[1]--p[2]
// 下面--abc
// p[4]--p[7]
// | |
// p[5]--p[6]
float xmin=m_range.getMinX()+gridSize*jmin;
float xmax=m_range.getMinX()+gridSize*jmax;
float zmin=m_range.getMinZ()+gridSize*imin;
float zmax=m_range.getMinZ()+gridSize*imax;
float ymin=m_range.getMinY();
float ymax=m_range.getMaxY();
float c[3]={(xmax+xmin)/2,(ymin+ymax)/2,(zmin+zmax)/2};
float r=max(xmax-xmin,ymax-ymin)*0.86602540378443864676372317075294;//由于zmax-zmin与xmax-xmin相等，所以不用考虑--abc
//看球体(c,r)是否都在planeList中某个面的反面，如果是则可剔除--abc
bool visible=true;
for(int i=0;i<5;i++){//不考虑远平面--abc
const Cc3dPlane&plane=this->getMesh()->getSubMeshByIndex(0)->getCamera()->getFrustum().getPlaneByIndex(i);
//看球体(c,r)是否在plane的背面--abc
float PND=directedDistanceFromPointToPlane(plane, c);
if(PND<-r){//如果在背面--abc
//断定为不可见，不用再继续检测--abc
visible=false;
break;
}
}//得到visible
if(visible){//如果可见--abc
bool needDiv=false;//是否需要再分--abc
//求needDiv
if(imin+1==imax){//无须再分，因为已经无法再分--abc
needDiv=false;
}else{//进一步判断--abc
//求c到视点的距离--abc
//指数值越大，LOD效应越明显--abc
float d=square(this->getMesh()->getSubMeshByIndex(0)->getCamera()->getEyePos().x()-c[0])
+square(minf(0.6*(this->getMesh()->getSubMeshByIndex(0)->getCamera()->getEyePos().y()-c[1]),700))//Y乘以一个比1小的系数是为了让LOD对高度变化不敏感--abc
+square(this->getMesh()->getSubMeshByIndex(0)->getCamera()->getEyePos().z()-c[2]);
float e=xmax-xmin;//边长--abc
if(d<e*reso)needDiv=true;
}//得到needDiv
if(needDiv){//继续分--abc
int imid=(imin+imax)>>1;//除2
int jmid=(jmin+jmax)>>1;
markmat[imid][jmid]=true;
markedElementIndexList.push_back(Cij(imid,jmid));
//对四个孩子继续递归--abc
getBlocks(jmin,jmid,imin,imid,curDepth+1);
getBlocks(jmin,jmid,imid,imax,curDepth+1);
getBlocks(jmid,jmax,imid,imax,curDepth+1);
getBlocks(jmid,jmax,imin,imid,curDepth+1);
}else{//不分--abc
CterrainBlock block(imin,imax,jmin,jmax);
m_blockList.push_back(block);
}
}
// 直線p1-p2と点qの距離
Real dist(P p1, P p2, P q) {
q = q-p1;
p2 = p2-p1;
return sqrt((q.dot(q)*p2.dot(p2) - square(q.dot(p2))) / p2.dot(p2));
}
void MayaCamera::Update(float timestep)
{
(void)timestep;
// Track mouse motion
int2 mousePos;
GetCursorPos((POINT *)&mousePos);
int2 mouseMove = mousePos - m_mousePosPrev;
m_mousePosPrev = mousePos;
// Handle mouse rotation
if (m_mbuttonCur == MBUTTON_Left)
{
m_yaw -= m_rotateSpeed * mouseMove.x;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch -= m_rotateSpeed * mouseMove.y;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
// Retrieve controller state
XINPUT_STATE controllerState = {};
float2 controllerLeftStick(0.0f), controllerRightStick(0.0f);
float controllerLeftTrigger = 0.0f, controllerRightTrigger = 0.0f;
if (m_controllerPresent)
{
// Look out for disconnection
if (XInputGetState(0, &controllerState) == ERROR_SUCCESS)
{
// Decode axes and apply dead zones
controllerLeftTrigger = float(max(0, controllerState.Gamepad.bLeftTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerRightTrigger = float(max(0, controllerState.Gamepad.bRightTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerLeftStick = float2(controllerState.Gamepad.sThumbLX, controllerState.Gamepad.sThumbLY);
float lengthLeft = length(controllerLeftStick);
if (lengthLeft > XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE)
controllerLeftStick = (controllerLeftStick / lengthLeft) * (lengthLeft - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
else
controllerLeftStick = float2(0.0f);
controllerRightStick = float2(controllerState.Gamepad.sThumbRX, controllerState.Gamepad.sThumbRY);
float lengthRight = length(controllerRightStick);
if (lengthRight > XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE)
controllerRightStick = (controllerRightStick / lengthRight) * (lengthRight - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);
else
controllerRightStick = float2(0.0f);
}
else
{
m_controllerPresent = false;
}
}
// Handle controller rotation
if (m_controllerPresent)
{
m_yaw -= m_controllerRotateSpeed * controllerRightStick.x * abs(controllerRightStick.x) * timestep;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch += m_controllerRotateSpeed * controllerRightStick.y * abs(controllerRightStick.y) * timestep;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
UpdateOrientation();
// Handle zoom
if (m_mbuttonCur == MBUTTON_Right)
{
m_radius *= expf(mouseMove.y * m_zoomSpeed);
}
m_radius *= expf(-m_wheelDelta * m_zoomWheelSpeed);
m_wheelDelta = 0;
// Handle controller zoom
if (m_controllerPresent && !(controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
{
m_radius *= expf(-controllerLeftStick.y * abs(controllerLeftStick.y) * m_controllerZoomSpeed * timestep);
}
// Handle motion of target point
if (m_mbuttonCur == MBUTTON_Middle)
{
m_posTarget -= m_rotateSpeed * mouseMove.x * m_radius * m_viewToWorld[0].xyz;
m_posTarget += m_rotateSpeed * mouseMove.y * m_radius * m_viewToWorld[1].xyz;
}
// Handle controller motion of target point
if (m_controllerPresent && (controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
{
float3 localVelocity(0.0f);
localVelocity.x = controllerLeftStick.x * abs(controllerLeftStick.x);
localVelocity.y = square(controllerRightTrigger) - square(controllerLeftTrigger);
localVelocity.z = -controllerLeftStick.y * abs(controllerLeftStick.y);
m_posTarget += xfmVector(localVelocity, m_viewToWorld) * (m_radius * m_controllerMoveSpeed * timestep);
}
// Calculate remaining matrices
m_pos = m_posTarget + m_radius * m_viewToWorld[2].xyz;
setTranslation(&m_viewToWorld, m_pos);
UpdateWorldToClip();
}
void FPSCamera::Update(float timestep)
{
// Track mouse motion
int2 mousePos;
GetCursorPos((POINT *)&mousePos);
int2 mouseMove = mousePos - m_mousePosPrev;
m_mousePosPrev = mousePos;
// Handle mouse rotation
if (m_mbuttonActivate == MBUTTON_None ||
m_mbuttonCur == m_mbuttonActivate)
{
m_yaw -= m_rotateSpeed * mouseMove.x;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch -= m_rotateSpeed * mouseMove.y;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
// Retrieve controller state
XINPUT_STATE controllerState = {};
float2 controllerLeftStick(0.0f), controllerRightStick(0.0f);
float controllerLeftTrigger = 0.0f, controllerRightTrigger = 0.0f;
if (m_controllerPresent)
{
// Look out for disconnection
if (XInputGetState(0, &controllerState) == ERROR_SUCCESS)
{
// Decode axes and apply dead zones
controllerLeftTrigger = float(max(0, controllerState.Gamepad.bLeftTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerRightTrigger = float(max(0, controllerState.Gamepad.bRightTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
controllerLeftStick = float2(controllerState.Gamepad.sThumbLX, controllerState.Gamepad.sThumbLY);
float lengthLeft = length(controllerLeftStick);
if (lengthLeft > XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE)
controllerLeftStick = (controllerLeftStick / lengthLeft) * (lengthLeft - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
else
controllerLeftStick = float2(0.0f);
controllerRightStick = float2(controllerState.Gamepad.sThumbRX, controllerState.Gamepad.sThumbRY);
float lengthRight = length(controllerRightStick);
if (lengthRight > XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE)
controllerRightStick = (controllerRightStick / lengthRight) * (lengthRight - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);
else
controllerRightStick = float2(0.0f);
}
else
{
m_controllerPresent = false;
}
}
// Handle controller rotation
if (m_controllerPresent)
{
m_yaw -= m_controllerRotateSpeed * controllerRightStick.x * abs(controllerRightStick.x) * timestep;
m_yaw = modPositive(m_yaw, 2.0f*pi);
m_pitch += m_controllerRotateSpeed * controllerRightStick.y * abs(controllerRightStick.y) * timestep;
m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
}
UpdateOrientation();
// Handle translation
// !!!UNDONE: acceleration based on how long you've been holding the button,
// to make fine motions easier?
float moveStep = timestep * m_moveSpeed;
// !!!UNDONE: move keyboard tracking into an input system that respects focus, etc.
if (GetAsyncKeyState(VK_SHIFT) || (controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
moveStep *= 3.0f;
if (GetAsyncKeyState('W'))
m_pos -= m_viewToWorld[2].xyz * moveStep;
if (GetAsyncKeyState('S'))
m_pos += m_viewToWorld[2].xyz * moveStep;
if (GetAsyncKeyState('A'))
m_pos -= m_viewToWorld[0].xyz * moveStep;
if (GetAsyncKeyState('D'))
m_pos += m_viewToWorld[0].xyz * moveStep;
if (GetAsyncKeyState('E'))
m_pos += m_viewToWorld[1].xyz * moveStep;
if (GetAsyncKeyState('C'))
m_pos -= m_viewToWorld[1].xyz * moveStep;
if (m_controllerPresent)
{
float3 localVelocity(0.0f);
localVelocity.x = controllerLeftStick.x * abs(controllerLeftStick.x);
localVelocity.y = square(controllerRightTrigger) - square(controllerLeftTrigger);
localVelocity.z = -controllerLeftStick.y * abs(controllerLeftStick.y);
m_pos += xfmVector(localVelocity, m_viewToWorld) * (moveStep * m_controllerMoveSpeed);
}
// Calculate remaining matrices
setTranslation(&m_viewToWorld, m_pos);
UpdateWorldToClip();
}
void snova(int insx, int insy) {
int comdead, nqx, nqy, nsx, nsy, num, kldead, iscdead;
int nrmdead, npdead;
int insipient=0;
nsx = insx;
nsy = insy;
if (insy== 0) {
if (insx == 1) {
/* NOVAMAX being used */
nqx = probecx;
nqy = probecy;
}
else {
int stars = 0;
/* Scheduled supernova -- select star */
/* logic changed here so that we won't favor quadrants in top
left of universe */
for (nqx = 1; nqx<=8; nqx++) {
for (nqy = 1; nqy<=8; nqy++) {
stars += d.galaxy[nqx][nqy] % 10;
}
}
if (stars == 0) return; /* nothing to supernova exists */
num = Rand()*stars + 1;
for (nqx = 1; nqx<=8; nqx++) {
for (nqy = 1; nqy<=8; nqy++) {
num -= d.galaxy[nqx][nqy] % 10;
if (num <= 0) break;
}
if (num <=0) break;
}
#ifdef DEBUG
if (idebug) {
proutn("Super nova here?");
if (ja()==1) {
nqx = quadx;
nqy = quady;
}
}
#endif
}
if (nqx != quady || nqy != quady || justin != 0) {
/* it isn't here, or we just entered (treat as inroute) */
if (REPORTS) {
skip(1);
proutn("Message from Starfleet Command Stardate ");
cramf(d.date, 0, 1);
skip(1);
proutn(" Supernova in");
cramlc(1, nqx, nqy);
prout("; caution advised.");
}
}
else {
/* we are in the quadrant! */
insipient = 1;
num = Rand()* (d.galaxy[nqx][nqy]%10) + 1;
for (nsx=1; nsx < 10; nsx++) {
for (nsy=1; nsy < 10; nsy++) {
if (quad[nsx][nsy]==IHSTAR) {
num--;
if (num==0) break;
}
}
if (num==0) break;
}
}
}
else {
insipient = 1;
}
if (insipient) {
skip(1);
prouts("***RED ALERT! RED ALERT!");
skip(1);
proutn("***Incipient supernova detected at");
cramlc(2, nsx, nsy);
skip(1);
nqx = quadx;
nqy = quady;
if (square(nsx-sectx) + square(nsy-secty) <= 2.1) {
proutn("Emergency override attempts t");
prouts("***************");
skip(1);
stars();
alldone=1;
}
}
/* destroy any Klingons in supernovaed quadrant */
num=d.galaxy[nqx][nqy];
kldead = num/100;
d.remkl -= kldead; // Moved here to correctly set remaining Klingon count
comdead = iscdead = 0;
if (nqx==d.isx && nqy == d.isy) {
/* did in the Supercommander! */
d.nscrem = d.isx = d.isy = isatb = iscate = 0;
iscdead = 1;
kldead--; /* Get proper kill credit */
future[FSCMOVE] = future[FSCDBAS] = 1e30;
}
if (d.remcom) {
int maxloop = d.remcom, l;
for (l = 1; l <= maxloop; l++) {
if (d.cx[l] == nqx && d.cy[l] == nqy) {
d.cx[l] = d.cx[d.remcom];
d.cy[l] = d.cy[d.remcom];
d.cx[d.remcom] = d.cy[d.remcom] = 0;
d.remcom--;
kldead--;
comdead++;
if (d.remcom==0) future[FTBEAM] = 1e30;
break;
}
}
}
/* destroy Romulans and planets in supernovaed quadrant */
num = d.newstuf[nqx][nqy];
d.newstuf[nqx][nqy] = 0;
nrmdead = num/10;
d.nromrem -= nrmdead;
npdead = num - nrmdead*10;
if (npdead) {
int l;
for (l = 1; l <= inplan; l++)
if (d.plnets[l].x == nqx && d.plnets[l].y == nqy) {
d.plnets[l] = nulplanet;
}
}
/* Destroy any base in supernovaed quadrant */
if (d.rembase) {
int maxloop = d.rembase, l;
for (l = 1; l <= maxloop; l++)
if (d.baseqx[l]==nqx && d.baseqy[l]==nqy) {
d.baseqx[l] = d.baseqx[d.rembase];
d.baseqy[l] = d.baseqy[d.rembase];
d.baseqx[d.rembase] = d.baseqy[d.rembase] = 0;
d.rembase--;
break;
}
}
/* If starship caused supernova, tally up destruction */
if (insx) {
num = d.galaxy[nqx][nqy] % 100;
d.starkl += num % 10;
d.basekl += num/10;
d.killk += kldead;
d.killc += comdead;
d.nromkl += nrmdead;
d.nplankl += npdead;
d.nsckill += iscdead;
}
/* mark supernova in galaxy and in star chart */
if ((quadx == nqx && quady == nqy) || REPORTS)
starch[nqx][nqy] = 1;
d.galaxy[nqx][nqy] = 1000;
/* If supernova destroys last klingons give special message */
if (d.remkl==0 && (nqx != quadx || nqy != quady)) {
skip(2);
if (insx == 0) prout("Lucky you!");
proutn("A supernova in");
cramlc(1, nqx, nqy);
prout(" has just destroyed the last Klingons.");
finish(FWON);
return;
}
/* if some Klingons remain, continue or die in supernova */
if (alldone) finish(FSNOVAED);
return;
}
bool within(entity* a, entity* b, float d) {
return a && b && (length_sq(a->_pos - b->_pos) < square(d));
}
//! Returns the square of the distance between two points.
inline double distanceSqr(double x1, double y1, double x2, double y2)
{
return square(x1 - x2) + square(y1 - y2);
}
bool lfLens::InterpolateVignetting (
float focal, float aperture, float distance, lfLensCalibVignetting &res) const
{
if (!CalibVignetting)
return false;
/* This is a pretty complex problem, despite its apparent simplicity.
* What we have is to find the intensity of a 3D force field in given
* point (let's call it p) determined by coordinates (X=focal,
* Y=aperture, Z=distance). For this we have a lot of measurements
* (e.g. calibration data) taken at other random points with their
* own X/Y/Z coordinates.
*
* We must find the line that intersects at least two calibration
* points and comes as close to the interpolation point as possible.
* Even better if we find 4 such points, in this case we can approximate
* by a real spline instead of doing linear interpolation.
*
* We will do this as follows. For every point find the other points
* on the line that intersects this point and the point p. We need
* just four points: two on one side of p, and two on other side of it.
* Then build a Hermit spline that passes through these four points.
* Finally, compute a estimate of the "quality" of this spline, which
* is made of: total spline length (the shorter is spline, the higher
* is quality), how close the resulting X,Y,Z is to our target.
* When we get a estimate of a high enough quality, we return
* the found point to the caller.
*
* Otherwise, the found point is added to the pool and the loop goes
* over and over again.
*/
bool rc = false;
GPtrArray *vc = g_ptr_array_new ();
int cvc;
for (cvc = 0; CalibVignetting [cvc]; cvc++)
g_ptr_array_add (vc, CalibVignetting [cvc]);
lfVignettingModel vm = LF_VIGNETTING_MODEL_NONE;
float min_dist = 0.01F;
for (guint i = 0; i < vc->len; i++)
{
lfLensCalibVignetting *c =
(lfLensCalibVignetting *)g_ptr_array_index (vc, i);
// Take into account just the first encountered lens model
if (vm == LF_VIGNETTING_MODEL_NONE)
vm = c->Model;
else if (vm != c->Model)
{
g_warning ("WARNING: lens %s/%s has multiple vignetting models defined\n",
Maker, Model);
continue;
}
// __
// Compute the vector pc
float pcx = c->Focal - focal;
float pcy = c->Aperture - aperture;
float pcz = c->Distance - distance;
float norm = sqrt (pcx * pcx + pcy * pcy + pcz * pcz);
// If c == p, return it without any interpolation
if (norm < 0.0001)
{
res = *c;
rc = true;
goto leave;
}
norm = 1.0 / norm;
pcx *= norm;
pcy *= norm;
pcz *= norm;
union
{
lfLensCalibVignetting *spline [4];
void *spline_ptr [4];
};
// Don't pick up way off points
float spline_rating [4] = { -10.0, -10.0, 1.0, 10.0 };
float spline_dist [4] = { FLT_MAX, FLT_MAX, 1.0, FLT_MAX };
memset (&spline, 0, sizeof (spline));
for (guint j = 0; j < vc->len; j++)
{
if (j == i)
continue;
lfLensCalibVignetting *x =
(lfLensCalibVignetting *)g_ptr_array_index (vc, j);
// __
// Calculate the vector px and bring it to same scale
float pxx = x->Focal - focal;
float pxy = x->Aperture - aperture;
float pxz = x->Distance - distance;
float norm2 = 1.0 / sqrt (pxx * pxx + pxy * pxy + pxz * pxz);
// __ __
// Compute the cosinus of the angle between the vectors px and pc
float cs = (pxx * pcx + pxy * pcy + pxz * pcz) * norm2;
if (cs > -0.01 && cs < +0.01)
continue; // +-90 degrees
// We will judge how good this point is for us by computing
// the rating as the relation distance/cos(angle)^3
float dist = sqrt (square (pxx * norm) + square (pxy * norm) + square (pxz * norm));
float rating = dist / (cs * cs * cs);
if (rating >= -0.00001 && dist <= +0.00001)
{
res = *x;
rc = true;
goto leave;
}
switch (__insert_spline (spline_ptr, spline_rating, rating, x))
{
case 0:
spline_dist [0] = dist;
break;
case 1:
spline_dist [0] = spline_dist [1];
spline_dist [1] = dist;
break;
case 2:
spline_dist [3] = spline_dist [2];
spline_dist [2] = dist;
break;
case 3:
spline_dist [3] = dist;
break;
}
}
// If we have found no points for the spline, drop
if (!spline [1] || !spline [2])
continue;
// Sort the spline points according to the real distance
// between p and the points, not by "rating".
if (spline_dist [0] < spline_dist [1])
{
lfLensCalibVignetting *tmp = spline [0];
spline [0] = spline [1];
spline [1] = tmp;
float tmpf = spline_dist [0];
spline_dist [0] = spline_dist [1];
spline_dist [1] = tmpf;
}
if (spline_dist [3] < spline_dist [2])
{
lfLensCalibVignetting *tmp = spline [2];
spline [2] = spline [3];
spline [3] = tmp;
float tmpf = spline_dist [2];
spline_dist [2] = spline_dist [3];
spline_dist [3] = tmpf;
}
// Interpolate a new point given four spline points
// For this we have to find first the 't' parameter
// in the range 0..1 which gives the closest to p point
lfLensCalibVignetting m;
float t = 0.5;
float w = 0.25;
while (w > 0.001)
{
__vignetting_interp (spline, m, t);
float md = __vignetting_dist (this, m, focal, aperture, distance);
if (md < 0.01)
break;
lfLensCalibVignetting m1;
__vignetting_interp (spline, m1, t + 0.01);
float md1 = __vignetting_dist (this, m1, focal, aperture, distance);
if (md1 > md)
t -= w;
else
t += w;
w /= 2;
}
for (size_t i = 0; i < ARRAY_LEN (res.Terms); i++)
m.Terms [i] = _lf_interpolate (
spline [0] ? spline [0]->Terms [i] : FLT_MAX,
spline [1]->Terms [i], spline [2]->Terms [i],
spline [3] ? spline [3]->Terms [i] : FLT_MAX, t);
m.Model = vm;
m.Focal = _lf_interpolate (
spline [0] ? spline [0]->Focal : FLT_MAX,
spline [1]->Focal, spline [2]->Focal,
spline [3] ? spline [3]->Focal : FLT_MAX, t);
m.Aperture = _lf_interpolate (
spline [0] ? spline [0]->Aperture : FLT_MAX,
spline [1]->Aperture, spline [2]->Aperture,
spline [3] ? spline [3]->Aperture : FLT_MAX, t);
m.Distance = _lf_interpolate (
spline [0] ? spline [0]->Distance : FLT_MAX,
spline [1]->Distance, spline [2]->Distance,
spline [3] ? spline [3]->Distance : FLT_MAX, t);
// If interpolated point is close enough, take it
float dist = __vignetting_dist (this, m, focal, aperture, distance);
// 0.005 is a carefully manually crafted value ;-D
if (dist < 0.005)
{
res = m;
rc = true;
goto leave;
}
if (dist < min_dist)
{
res = m;
rc = true;
}
g_ptr_array_add (vc, new lfLensCalibVignetting (m));
}
leave:
for (guint i = cvc; i < vc->len; i++)
{
lfLensCalibVignetting *c =
(lfLensCalibVignetting *)g_ptr_array_index (vc, i);
delete c;
}
g_ptr_array_free (vc, TRUE);
return rc;
}
int main()
{
long long T,A,B,t1,t2;
double a1=0,a2=0,ans=0;
scanf("%lld",&T);
while(T--)
{
scanf("%lld %lld %lld %lld",&A,&B,&t1,&t2);
if(A==B)
{
square(A,t1,t2);
}
else if(A>B) //if T1>T2. Then T2 OR B will be in the X Axis
{
if((B+t2)<A)
{
a1=0.5*((A-(B+t2))+(A-t2))*B;
}
else
{
a1=0.5*(A-t2)*(A-t2);
}
a2=0.5*(B-t1)*(B-t1);
if(t1>B)
{
a2=0;
}
if(t2>A)
{
a1=0;
}
ans=(A*B)-(a1+a2);
ans=ans/(A*B);
printf("%f\n",ans);
}
else if(B>A)
{
a2=0.5*(A-t2)*(A-t2);
if((A+t1)<B)
{
a1=0.5*((B-(A+t1))+(B-t1))*A;
}
else
{
a1=0.5*(B-t1)*(B-t1);
}
if(t2>=A)
{
a2=0;
}
if(t1>=B)
{
a1=0;
}
ans=(A*B)-(a1+a2);
ans=ans/(A*B);
printf("%f\n",ans);
}
}
return 0;
}
int main (int argc, char *argv[]){
int i, j, k;
if(argc != 3){
printf("failed, need input arguments\n");
return;
}
/*******************************************************
* read Gaussian data generated from readAO.pl script *
*******************************************************/
FILE *fCMNBasis;
FILE *frCrd, *fCTRC;
FILE *fEVa, *fEWa, *fACHARGE;
FILE *fGmo, *fTCrd;
int tmp;
/* manipulating input strings */
char *TCrd;
char *gmoIn=strdup(argv[1]);
TCrd=strtok(gmoIn, ".");
strcat(TCrd,".crd");
/* read files from perl and python script */
fCMNBasis = fopen("CMNBasis.pltmp","r");
frCrd = fopen("COORD.pltmp","r");
fEVa = fopen("EV_a.pltmp","r");
fEWa = fopen("EW_a.pltmp","r");
fGmo = fopen(argv[1],"r");
fTCrd = fopen(TCrd,"r");
fACHARGE = fopen("ACHARGE.pltmp","r");
for(i=0;i<3;i++){
fscanf(fCMNBasis,"%d",&tmp);
}
fscanf(fCMNBasis,"%d",&HOMOa);
fscanf(fCMNBasis,"%d",&HOMOb);
fscanf(fCMNBasis,"%d",&Nbasis);
fscanf(fCMNBasis,"%d",&Ngaussian);
fscanf(fCMNBasis,"%d",&Nshell);
fscanf(fCMNBasis,"%d",&Natom);
/* number of atoms in target system */
/* need to be rewrite for alchemy */
/* may be done by read until EOF */
/* specific format of crd file needed */
{
FILE *crdTmp = fopen(TCrd,"r");
char *string;
size_t len=0;
string = (char*) malloc(80*sizeof(char));
while(!feof(crdTmp)){
getline(&string,&len,crdTmp);
TNatom++;
}
TNatom--;
}
//printf("%d\n",TNatom);
//printf("%d\t%d\t%d\n",Nbasis,Ngaussian,Nshell);
EWa = (double *) malloc(Nbasis * sizeof(double));
EVa = (double *) malloc(Nbasis*Nbasis * sizeof(double));
Gmo = (double *) malloc(Nbasis*Nbasis * sizeof(double));
rCrg = (int *) malloc(Natom * sizeof(int));
rCrd = (double *) malloc(3 * Natom * sizeof(double));
tCrg = (int *) malloc(TNatom * sizeof(int));
tCrd = (double *) malloc(3 * TNatom * sizeof(double));
for(i=0;i<Nbasis;i++){
fscanf(fEWa,"%lf",&EWa[i]);
for(j=0;j<Nbasis;j++){
int s = i*Nbasis + j;
fscanf(fEVa,"%lf",&EVa[s]);
fscanf(fGmo,"%lf",&Gmo[s]);
}
}
for(i=0;i<Natom;i++){
double tmp;
/* ACHARGE is written in scientific notation */
fscanf(fACHARGE,"%lf",&tmp);
rCrg[i] = tmp;
fscanf(frCrd,"%lf",&rCrd[i*3]);
fscanf(frCrd,"%lf",&rCrd[i*3+1]);
fscanf(frCrd,"%lf",&rCrd[i*3+2]);
}
for(i=0;i<TNatom;i++){
fscanf(fTCrd,"%d",&tCrg[i]);
fscanf(fTCrd,"%lf",&tCrd[i*3]);
fscanf(fTCrd,"%lf",&tCrd[i*3+1]);
fscanf(fTCrd,"%lf",&tCrd[i*3+2]);
}
//printf("%d %d\n",tCrg[0],rCrg[0]);
for(i=0;i<Natom;i++){
for(j=i+1;j<Natom;j++){
double Rij = 0;
Rij += square(rCrd[i*3]-rCrd[j*3]);
Rij += square(rCrd[i*3+1]-rCrd[j*3+1]);
Rij += square(rCrd[i*3+2]-rCrd[j*3+2]);
Vr += rCrg[i]*rCrg[j]/sqrt(Rij);
}
}
for(i=0;i<TNatom;i++){
for(j=i+1;j<TNatom;j++){
double Rij = 0;
Rij += square(tCrd[i*3]-tCrd[j*3]);
Rij += square(tCrd[i*3+1]-tCrd[j*3+1]);
Rij += square(tCrd[i*3+2]-tCrd[j*3+2]);
Vt += tCrg[i]*tCrg[j]/sqrt(Rij);
}
}
dVN = Vt - Vr;
//printf("% 14.7E % 14.7E % 14.7E\n",Vr, Vt, dVN);
/****************END of reading*****************/
int evi, evj, evk, eva, evb, evc;
double d1E = 0;
for(evi=0;evi<HOMOa;evi++){
int ii = evi*Nbasis + evi;
d1E += Gmo[ii];
}
for(evi=0;evi<HOMOb;evi++){
int ii = evi*Nbasis + evi;
d1E += Gmo[ii];
}
d1E += dVN;
double d2E = 0;
for(evi=0;evi<HOMOa;evi++){
for(eva=HOMOa;eva<Nbasis;eva++){
int ia = evi*Nbasis + eva;
double Cia = EWa[evi] - EWa[eva];
d2E += square(Gmo[ia]) / Cia;
}
}
for(evi=0;evi<HOMOb;evi++){
for(eva=HOMOb;eva<Nbasis;eva++){
int ia = evi*Nbasis + eva;
double Cia = EWa[evi] - EWa[eva];
d2E += square(Gmo[ia]) / Cia;
}
}
/**************
* 3rd order *
**************/
double d3E = 0;
for(evi=0;evi<HOMOa;evi++){
for(eva=HOMOa;eva<Nbasis;eva++){
int ia = evi*Nbasis + eva;
double Cia = EWa[evi] - EWa[eva];
for(evb=HOMOa;evb<Nbasis;evb++){
int ib = evi*Nbasis + evb;
int ab = eva*Nbasis + evb;
double Cib = EWa[evi] - EWa[evb];
double Ciab = Cia*Cib;
d3E += Gmo[ia]*Gmo[ib]*Gmo[ab] / Ciab;
}
for(evj=0;evj<HOMOa;evj++){
int ja = evj*Nbasis + eva;
int ij = evj*Nbasis + evj;
double Cja = EWa[evj] - EWa[eva];
double Cija = Cia*Cja;
d3E -= Gmo[ia]*Gmo[ja]*Gmo[ij] / Cija;
}
}
}
for(evi=0;evi<HOMOb;evi++){
for(eva=HOMOb;eva<Nbasis;eva++){
int ia = evi*Nbasis + eva;
double Cia = EWa[evi] - EWa[eva];
for(evb=HOMOb;evb<Nbasis;evb++){
int ib = evi*Nbasis + evb;
int ab = eva*Nbasis + evb;
double Cib = EWa[evi] - EWa[evb];
double Ciab = Cia*Cib;
d3E += Gmo[ia]*Gmo[ib]*Gmo[ab] / Ciab;
}
for(evj=0;evj<HOMOb;evj++){
int ja = evj*Nbasis + eva;
int ij = evj*Nbasis + evj;
double Cja = EWa[evj] - EWa[eva];
double Cija = Cia*Cja;
d3E -= Gmo[ia]*Gmo[ja]*Gmo[ij] / Cija;
}
}
}
/**************
* 4th order *
**************/
double d4E = 0;
for(evi=0;evi<HOMOa;evi++){
for(eva=HOMOa;eva<Nbasis;eva++){
for(evb=HOMOa;evb<Nbasis;evb++){
for(evc=HOMOa;evc<Nbasis;evc++){
int ia = evi*Nbasis + eva;
int ib = evi*Nbasis + evb;
int ab = eva*Nbasis + evb;
int bc = evb*Nbasis + evc;
double Cia = EWa[evi] - EWa[eva];
double Cib = EWa[evi] - EWa[evb];
double Cic = EWa[evi] - EWa[evc];
double V = Gmo[ia]*Gmo[ib]*Gmo[ab]*Gmo[bc];
double E = Cia*Cib*Cic;
d4E += V/E;
}
}
}
}
for(evi=0;evi<HOMOa;evi++){
for(evj=0;evj<HOMOa;evj++){
for(evk=0;evk<HOMOa;evk++){
for(eva=HOMOa;eva<Nbasis;eva++){
int ia = evi*Nbasis + eva;
int ja = evj*Nbasis + eva;
int ik = evi*Nbasis + evk;
int jk = evj*Nbasis + evk;
double Cia = EWa[evi] - EWa[eva];
double Cja = EWa[evj] - EWa[eva];
double Cka = EWa[evk] - EWa[eva];
double V = Gmo[ia]*Gmo[ja]*Gmo[ik]*Gmo[jk];
double E = Cia*Cja*Cka;
d4E += V/E;
}
}
}
}
for(evi=0;evi<HOMOa;evi++){
for(evj=0;evj<HOMOa;evj++){
for(eva=HOMOa;eva<Nbasis;eva++){
for(evb=HOMOa;evb<Nbasis;evb++){
int ia = evi*Nbasis + eva;
int ib = evi*Nbasis + evb;
int jb = evj*Nbasis + evb;
int ij = evi*Nbasis + evk;
int ab = evj*Nbasis + evk;
double Cia = EWa[evi] - EWa[eva];
double Cjb = EWa[evj] - EWa[evb];
double Cib = EWa[evi] - EWa[evb];
double Cij = EWa[evi] + EWa[evj];
double Cab = EWa[eva] + EWa[evb];
double V = Gmo[ia]*Gmo[jb]*Gmo[ij]*Gmo[ab];
double E = Cia*Cjb*Cib;
d4E -= 2*V/E;
double V1 = Gmo[ia]*Gmo[jb]*Gmo[ib]*Gmo[ia];
double E1 = Cia*Cjb*(Cij - Cab);
double E2 = Cia*Cjb*Cjb;
d4E += (V1/E1 -V1/E2);
}
}
}
}
d4E *= 2;
// printf("% 10.7lf\n",d1E);
// printf("% 10.7lf % 10.7lf\n",d1E, d2E);
// printf("% 10.7lf % 10.7lf % 10.7f\n",d1E, d2E, d3E);
printf("% 10.7lf % 10.7lf % 10.7lf % 10.7f\n",d1E, d2E, d3E, d4E);
free(EVa);
free(EWa);
free(Gmo);
free(tCrg);
free(rCrg);
free(tCrd);
free(rCrd);
fcloseall();
return 0;
}
void curve25519_athlon_recip(double out[10],const double z[10])
{
double z2[10];
double z9[10];
double z11[10];
double z2_5_0[10];
double z2_10_0[10];
double z2_20_0[10];
double z2_50_0[10];
double z2_100_0[10];
double t0[10];
double t1[10];
int i;
/* 2 */ square(z2,z);
/* 4 */ square(t1,z2);
/* 8 */ square(t0,t1);
/* 9 */ mult(z9,t0,z);
/* 11 */ mult(z11,z9,z2);
/* 22 */ square(t0,z11);
/* 2^5 - 2^0 = 31 */ mult(z2_5_0,t0,z9);
/* 2^6 - 2^1 */ square(t0,z2_5_0);
/* 2^7 - 2^2 */ square(t1,t0);
/* 2^8 - 2^3 */ square(t0,t1);
/* 2^9 - 2^4 */ square(t1,t0);
/* 2^10 - 2^5 */ square(t0,t1);
/* 2^10 - 2^0 */ mult(z2_10_0,t0,z2_5_0);
/* 2^11 - 2^1 */ square(t0,z2_10_0);
/* 2^12 - 2^2 */ square(t1,t0);
/* 2^20 - 2^10 */ for (i = 2; i < 10; i += 2) {
square(t0,t1);
square(t1,t0);
}
/* 2^20 - 2^0 */ mult(z2_20_0,t1,z2_10_0);
/* 2^21 - 2^1 */ square(t0,z2_20_0);
/* 2^22 - 2^2 */ square(t1,t0);
/* 2^40 - 2^20 */ for (i = 2; i < 20; i += 2) {
square(t0,t1);
square(t1,t0);
}
/* 2^40 - 2^0 */ mult(t0,t1,z2_20_0);
/* 2^41 - 2^1 */ square(t1,t0);
/* 2^42 - 2^2 */ square(t0,t1);
/* 2^50 - 2^10 */ for (i = 2; i < 10; i += 2) {
square(t1,t0);
square(t0,t1);
}
/* 2^50 - 2^0 */ mult(z2_50_0,t0,z2_10_0);
/* 2^51 - 2^1 */ square(t0,z2_50_0);
/* 2^52 - 2^2 */ square(t1,t0);
/* 2^100 - 2^50 */ for (i = 2; i < 50; i += 2) {
square(t0,t1);
square(t1,t0);
}
/* 2^100 - 2^0 */ mult(z2_100_0,t1,z2_50_0);
/* 2^101 - 2^1 */ square(t1,z2_100_0);
/* 2^102 - 2^2 */ square(t0,t1);
/* 2^200 - 2^100 */ for (i = 2; i < 100; i += 2) {
square(t1,t0);
square(t0,t1);
}
/* 2^200 - 2^0 */ mult(t1,t0,z2_100_0);
/* 2^201 - 2^1 */ square(t0,t1);
/* 2^202 - 2^2 */ square(t1,t0);
/* 2^250 - 2^50 */ for (i = 2; i < 50; i += 2) {
square(t0,t1);
square(t1,t0);
}
/* 2^250 - 2^0 */ mult(t0,t1,z2_50_0);
/* 2^251 - 2^1 */ square(t1,t0);
/* 2^252 - 2^2 */ square(t0,t1);
/* 2^253 - 2^3 */ square(t1,t0);
/* 2^254 - 2^4 */ square(t0,t1);
/* 2^255 - 2^5 */ square(t1,t0);
/* 2^255 - 21 */ mult(out,t1,z11);
}
static void plot_constellations(cairo_t* cairo, plot_args_t* pargs, plotann_t* ann) {
int i, N;
double ra,dec,radius;
double xyzf[3];
// Find the field center and radius
anwcs_get_radec_center_and_radius(pargs->wcs, &ra, &dec, &radius);
logverb("Plotting constellations: field center %g,%g, radius %g\n",
ra, dec, radius);
radecdeg2xyzarr(ra, dec, xyzf);
radius = deg2dist(radius);
N = constellations_n();
for (i=0; i<N; i++) {
int j, k;
// Find the approximate center and radius of this constellation
// and see if it overlaps with the field.
il* stars = constellations_get_unique_stars(i);
double xyzj[3];
double xyzc[3];
double maxr2 = 0;
dl* rds;
xyzc[0] = xyzc[1] = xyzc[2] = 0.0;
xyzj[0] = xyzj[1] = xyzj[2] = 0.0;
for (j=0; j<il_size(stars); j++) {
constellations_get_star_radec(il_get(stars, j), &ra, &dec);
radecdeg2xyzarr(ra, dec, xyzj);
for (k=0; k<3; k++)
xyzc[k] += xyzj[k];
}
normalize_3(xyzc);
for (j=0; j<il_size(stars); j++) {
constellations_get_star_radec(il_get(stars, j), &ra, &dec);
maxr2 = MAX(maxr2, distsq(xyzc, xyzj, 3));
}
il_free(stars);
maxr2 = square(sqrt(maxr2) + radius);
if (distsq(xyzf, xyzc, 3) > maxr2) {
xyzarr2radecdeg(xyzc, &ra, &dec);
logverb("Constellation %s (center %g,%g, radius %g) out of bounds\n",
constellations_get_shortname(i), ra, dec,
dist2deg(sqrt(maxr2) - radius));
logverb(" dist from field center to constellation center is %g deg\n",
distsq2deg(distsq(xyzf, xyzc, 3)));
logverb(" max radius: %g\n", distsq2deg(maxr2));
continue;
}
if (ann->constellation_pastel) {
float r,g,b;
xyzarr2radecdeg(xyzc, &ra, &dec);
color_for_radec(ra, dec, &r,&g,&b);
plotstuff_set_rgba2(pargs, r,g,b, 0.8);
plotstuff_builtin_apply(cairo, pargs);
}
// Phew, plot it.
if (ann->constellation_lines) {
rds = constellations_get_lines_radec(i);
logverb("Constellation %s: plotting %zu lines\n",
constellations_get_shortname(i), dl_size(rds)/4);
for (j=0; j<dl_size(rds)/4; j++) {
double r1,d1,r2,d2;
double r3,d3,r4,d4;
double off = ann->constellation_lines_offset;
r1 = dl_get(rds, j*4+0);
d1 = dl_get(rds, j*4+1);
r2 = dl_get(rds, j*4+2);
d2 = dl_get(rds, j*4+3);
if (anwcs_find_discontinuity(pargs->wcs, r1, d1, r2, d2,
&r3, &d3, &r4, &d4)) {
logverb("Discontinuous: %g,%g -- %g,%g\n", r1, d1, r2, d2);
logverb(" %g,%g == %g,%g\n", r3,d3, r4,d4);
plot_offset_line_rd(NULL, pargs, r1,d1,r3,d3, off, 0.);
plot_offset_line_rd(NULL, pargs, r4,d4,r2,d2, 0., off);
} else {
plot_offset_line_rd(NULL, pargs, r1,d1,r2,d2, off, off);
}
plotstuff_stroke(pargs);
}
dl_free(rds);
}
if (ann->constellation_labels ||
ann->constellation_markers) {
// Put the label at the center of mass of the stars that
// are in-bounds
int Nin = 0;
stars = constellations_get_unique_stars(i);
xyzc[0] = xyzc[1] = xyzc[2] = 0.0;
logverb("Labeling %s: %zu stars\n", constellations_get_shortname(i),
il_size(stars));
for (j=0; j<il_size(stars); j++) {
constellations_get_star_radec(il_get(stars, j), &ra, &dec);
if (!anwcs_radec_is_inside_image(pargs->wcs, ra, dec))
continue;
if (ann->constellation_markers)
plotstuff_marker_radec(pargs, ra, dec);
radecdeg2xyzarr(ra, dec, xyzj);
for (k=0; k<3; k++)
xyzc[k] += xyzj[k];
Nin++;
}
logverb(" %i stars in-bounds\n", Nin);
if (ann->constellation_labels && Nin) {
const char* label;
normalize_3(xyzc);
xyzarr2radecdeg(xyzc, &ra, &dec);
if (ann->constellation_labels_long)
label = constellations_get_longname(i);
else
label = constellations_get_shortname(i);
plotstuff_text_radec(pargs, ra, dec, label);
}
il_free(stars);
}
}
}
int main()
{
for (int i = 0; i<100; ++i)
cout << i<< '\1'<< square(i)<< '\n ';
}
// dissimilarity measure between pixels
static inline float diff(image<float> *r, image<float> *g, image<float> *b,
int x1, int y1, int x2, int y2) {
return sqrt(square(imRef(r, x1, y1)-imRef(r, x2, y2)) +
square(imRef(g, x1, y1)-imRef(g, x2, y2)) +
square(imRef(b, x1, y1)-imRef(b, x2, y2)));
}
double BoschBMA250::getVectorMagnitude()
{
double magnitude = sqrt(square(getAccelerationX()) + square(getAccelerationY()) + square(getAccelerationZ()));
return magnitude;
}
bool checkTarget(int a, int b, int c)
{
return square(a) + square(b) == square(c);
}
extern int setupBoard(Board_t self, const char *fen)
{
int ix = 0;
/*
* Squares
*/
while (isspace(fen[ix])) ix++;
int file = fileA, rank = rank8;
int nrWhiteKings = 0, nrBlackKings = 0;
memset(self->squares, empty, boardSize);
self->materialKey = 0;
while (rank != rank1 || file != fileH + fileStep) {
int piece = empty;
int count = 1;
switch (fen[ix]) {
case 'K': piece = whiteKing; nrWhiteKings++; break;
case 'Q': piece = whiteQueen; break;
case 'R': piece = whiteRook; break;
case 'B': piece = whiteBishop; break;
case 'N': piece = whiteKnight; break;
case 'P': piece = whitePawn; break;
case 'k': piece = blackKing; nrBlackKings++; break;
case 'r': piece = blackRook; break;
case 'q': piece = blackQueen; break;
case 'b': piece = blackBishop; break;
case 'n': piece = blackKnight; break;
case 'p': piece = blackPawn; break;
case '/': rank -= rankStep; file = fileA; ix++; continue;
case '1': case '2': case '3': case '4':
case '5': case '6': case '7': case '8':
count = fen[ix] - '0';
break;
default:
return 0; // FEN error
}
int squareColor = squareColor(square(file,rank));
self->materialKey += materialKeys[piece][squareColor];
do {
self->squares[square(file,rank)] = piece;
file += fileStep;
} while (--count && file != fileH + fileStep);
ix++;
}
if (nrWhiteKings != 1 || nrBlackKings != 1) return 0;
/*
* Side to move
*/
self->plyNumber = 2 + (fen[ix+1] == 'b'); // 2 means full move number starts at 1
//self->lastZeroing = self->plyNumber;
ix += 2;
/*
* Castling flags
*/
while (isspace(fen[ix])) ix++;
self->castleFlags = 0;
for (;; ix++) {
switch (fen[ix]) {
case 'K': self->castleFlags |= castleFlagWhiteKside; continue;
case 'Q': self->castleFlags |= castleFlagWhiteQside; continue;
case 'k': self->castleFlags |= castleFlagBlackKside; continue;
case 'q': self->castleFlags |= castleFlagBlackQside; continue;
case '-': ix++; break;
default: break;
}
break;
}
/*
* En passant square
*/
while (isspace(fen[ix])) ix++;
if ('a' <= fen[ix] && fen[ix] <= 'h') {
file = charToFile(fen[ix]);
ix++;
rank = (sideToMove(self) == white) ? rank5 : rank4;
if (isdigit(fen[ix])) ix++; // ignore what it says
self->enPassantPawn = square(file, rank);
} else {
self->enPassantPawn = 0;
if (fen[ix] == '-')
ix++;
}
// Eat move number and halfmove clock. TODO: process this properly
while (isspace(fen[ix])) ix++;
while (isdigit(fen[ix])) ix++;
while (isspace(fen[ix])) ix++;
while (isdigit(fen[ix])) ix++;
self->sideInfoPlyNumber = -1; // side info is invalid now
// Reset the undo stack
self->undoStack.len = 0;
// Initialize hash and its history
self->hash = hash(self);
self->hashHistory.len = 0;
self->pawnKingHash = pawnKingHash(self);
self->pkHashHistory.len = 0;
normalizeEnPassantStatus(self); // Only safe after update of hash
self->eloDiff = 0;
return ix;
}
void makePlot(TH1* histogram_data, bool doKeepBlinded,
TH1* histogram_ttH,
TH1* histogram_ttZ,
TH1* histogram_ttW,
TH1* histogram_EWK,
TH1* histogram_Rares,
TH1* histogram_fakes,
TH1* histogramSum_mc,
TH1* histogramErr_mc,
const std::string& xAxisTitle,
const std::string& yAxisTitle, double yMin, double yMax,
bool showLegend,
const std::string& label,
const std::string& outputFileName,
bool useLogScale)
{
TH1* histogram_data_density = 0;
if ( histogram_data ) {
histogram_data_density = divideHistogramByBinWidth(histogram_data);
}
histogram_data_density->SetMarkerColor(1);
histogram_data_density->SetMarkerStyle(20);
histogram_data_density->SetMarkerSize(2);
histogram_data_density->SetLineColor(1);
histogram_data_density->SetLineWidth(1);
histogram_data_density->SetLineStyle(1);
TH1* histogram_ttH_density = 0;
if ( histogram_ttH ) {
if ( histogram_data ) checkCompatibleBinning(histogram_ttH, histogram_data);
histogram_ttH_density = divideHistogramByBinWidth(histogram_ttH);
}
histogram_ttH_density->SetFillColor(628);
histogram_ttH_density->SetLineColor(1);
histogram_ttH_density->SetLineWidth(1);
TH1* histogram_ttZ_density = 0;
if ( histogram_ttZ ) {
if ( histogram_data ) checkCompatibleBinning(histogram_ttZ, histogram_data);
histogram_ttZ_density = divideHistogramByBinWidth(histogram_ttZ);
}
histogram_ttZ_density->SetFillColor(822);
histogram_ttZ_density->SetLineColor(1);
histogram_ttZ_density->SetLineWidth(1);
TH1* histogram_ttW_density = 0;
if ( histogram_ttW ) {
if ( histogram_data ) checkCompatibleBinning(histogram_ttW, histogram_data);
histogram_ttW_density = divideHistogramByBinWidth(histogram_ttW);
}
histogram_ttW_density->SetFillColor(823);
histogram_ttW_density->SetLineColor(1);
histogram_ttW_density->SetLineWidth(1);
TH1* histogram_EWK_density = 0;
if ( histogram_EWK ) {
if ( histogram_data ) checkCompatibleBinning(histogram_EWK, histogram_data);
histogram_EWK_density = divideHistogramByBinWidth(histogram_EWK);
}
histogram_EWK_density->SetFillColor(610);
histogram_EWK_density->SetLineColor(1);
histogram_EWK_density->SetLineWidth(1);
TH1* histogram_Rares_density = 0;
if ( histogram_Rares ) {
if ( histogram_data ) checkCompatibleBinning(histogram_Rares, histogram_data);
histogram_Rares_density = divideHistogramByBinWidth(histogram_Rares);
}
histogram_Rares_density->SetFillColor(851);
histogram_Rares_density->SetLineColor(1);
histogram_Rares_density->SetLineWidth(1);
TH1* histogram_fakes_density = 0;
if ( histogram_fakes ) {
if ( histogram_data ) checkCompatibleBinning(histogram_fakes, histogram_data);
histogram_fakes_density = divideHistogramByBinWidth(histogram_fakes);
}
histogram_fakes_density->SetFillColor(1);
histogram_fakes_density->SetFillStyle(3005);
histogram_fakes_density->SetLineColor(1);
histogram_fakes_density->SetLineWidth(1);
TH1* histogramSum_mc_density = 0;
if ( histogramSum_mc ) {
if ( histogram_data ) checkCompatibleBinning(histogramSum_mc, histogram_data);
histogramSum_mc_density = divideHistogramByBinWidth(histogramSum_mc);
}
std::cout << "histogramSum_mc_density = " << histogramSum_mc_density << std::endl;
dumpHistogram(histogramSum_mc_density);
TH1* histogramErr_mc_density = 0;
if ( histogramErr_mc ) {
if ( histogram_data ) checkCompatibleBinning(histogramErr_mc, histogram_data);
histogramErr_mc_density = divideHistogramByBinWidth(histogramErr_mc);
}
setStyle_uncertainty(histogramErr_mc_density);
TCanvas* canvas = new TCanvas("canvas", "canvas", 950, 1100);
canvas->SetFillColor(10);
canvas->SetBorderSize(2);
canvas->Draw();
TPad* topPad = new TPad("topPad", "topPad", 0.00, 0.34, 1.00, 0.995);
topPad->SetFillColor(10);
topPad->SetTopMargin(0.065);
topPad->SetLeftMargin(0.20);
topPad->SetBottomMargin(0.00);
topPad->SetRightMargin(0.04);
topPad->SetLogy(useLogScale);
TPad* bottomPad = new TPad("bottomPad", "bottomPad", 0.00, 0.01, 1.00, 0.335);
bottomPad->SetFillColor(10);
bottomPad->SetTopMargin(0.085);
bottomPad->SetLeftMargin(0.20);
bottomPad->SetBottomMargin(0.35);
bottomPad->SetRightMargin(0.04);
bottomPad->SetLogy(false);
canvas->cd();
topPad->Draw();
topPad->cd();
THStack* histogramStack_mc = new THStack();
histogramStack_mc->Add(histogram_fakes_density);
histogramStack_mc->Add(histogram_Rares_density);
histogramStack_mc->Add(histogram_EWK_density);
histogramStack_mc->Add(histogram_ttW_density);
histogramStack_mc->Add(histogram_ttZ_density);
histogramStack_mc->Add(histogram_ttH_density);
TH1* histogram_ref = histogram_data_density;
histogram_ref->SetTitle("");
histogram_ref->SetStats(false);
histogram_ref->SetMaximum(yMax);
histogram_ref->SetMinimum(yMin);
TAxis* xAxis_top = histogram_ref->GetXaxis();
assert(xAxis_top);
if ( xAxisTitle != "" ) xAxis_top->SetTitle(xAxisTitle.data());
xAxis_top->SetTitleOffset(1.20);
xAxis_top->SetLabelColor(10);
xAxis_top->SetTitleColor(10);
TAxis* yAxis_top = histogram_ref->GetYaxis();
assert(yAxis_top);
if ( yAxisTitle != "" ) yAxis_top->SetTitle(yAxisTitle.data());
yAxis_top->SetTitleOffset(1.20);
yAxis_top->SetTitleSize(0.080);
yAxis_top->SetLabelSize(0.065);
yAxis_top->SetTickLength(0.04);
histogram_ref->Draw("axis");
// CV: calling THStack::Draw() causes segmentation violation ?!
//histogramStack_mc->Draw("histsame");
// CV: draw histograms without using THStack instead;
// note that order in which histograms need to be drawn needs to be reversed
// compared to order in which histograms were added to THStack !!
histogram_ttH_density->Add(histogram_ttZ_density);
histogram_ttH_density->Add(histogram_ttW_density);
histogram_ttH_density->Add(histogram_EWK_density);
histogram_ttH_density->Add(histogram_Rares_density);
histogram_ttH_density->Add(histogram_fakes_density);
histogram_ttH_density->Draw("histsame");
std::cout << "histogram_ttH_density = " << histogram_ttH_density << ":" << std::endl;
dumpHistogram(histogram_ttH_density);
histogram_ttZ_density->Add(histogram_ttW_density);
histogram_ttZ_density->Add(histogram_EWK_density);
histogram_ttZ_density->Add(histogram_Rares_density);
histogram_ttZ_density->Add(histogram_fakes_density);
histogram_ttZ_density->Draw("histsame");
histogram_ttW_density->Add(histogram_EWK_density);
histogram_ttW_density->Add(histogram_Rares_density);
histogram_ttW_density->Add(histogram_fakes_density);
histogram_ttW_density->Draw("histsame");
histogram_EWK_density->Add(histogram_Rares_density);
histogram_EWK_density->Add(histogram_fakes_density);
histogram_EWK_density->Draw("histsame");
histogram_Rares_density->Add(histogram_fakes_density);
histogram_Rares_density->Draw("histsame");
TH1* histogram_fakes_density_cloned = (TH1*)histogram_fakes_density->Clone();
histogram_fakes_density_cloned->SetFillColor(10);
histogram_fakes_density_cloned->SetFillStyle(1001);
histogram_fakes_density_cloned->Draw("histsame");
histogram_fakes_density->Draw("histsame");
if ( histogramErr_mc_density ) {
histogramErr_mc_density->Draw("e2same");
}
if ( !doKeepBlinded ) {
histogram_data_density->Draw("e1psame");
}
histogram_ref->Draw("axissame");
double legend_y0 = 0.6950;
if ( showLegend ) {
TLegend* legend1 = new TLegend(0.2600, legend_y0, 0.5350, 0.9250, NULL, "brNDC");
legend1->SetFillStyle(0);
legend1->SetBorderSize(0);
legend1->SetFillColor(10);
legend1->SetTextSize(0.050);
TH1* histogram_data_forLegend = (TH1*)histogram_data_density->Clone();
histogram_data_forLegend->SetMarkerSize(2);
legend1->AddEntry(histogram_data_forLegend, "Observed", "p");
legend1->AddEntry(histogram_ttH_density, "t#bar{t}H", "f");
legend1->AddEntry(histogram_ttZ_density, "t#bar{t}Z", "f");
legend1->AddEntry(histogram_ttW_density, "t#bar{t}W", "f");
legend1->Draw();
TLegend* legend2 = new TLegend(0.6600, legend_y0, 0.9350, 0.9250, NULL, "brNDC");
legend2->SetFillStyle(0);
legend2->SetBorderSize(0);
legend2->SetFillColor(10);
legend2->SetTextSize(0.050);
legend2->AddEntry(histogram_EWK_density, "Electroweak", "f");
legend2->AddEntry(histogram_Rares_density, "Rares", "f");
legend2->AddEntry(histogram_fakes_density, "Fakes", "f");
if ( histogramErr_mc ) legend2->AddEntry(histogramErr_mc_density, "Uncertainty", "f");
legend2->Draw();
}
//addLabel_CMS_luminosity(0.2100, 0.9700, 0.6350);
addLabel_CMS_preliminary(0.2100, 0.9700, 0.6350);
TPaveText* label_category = 0;
if ( showLegend ) label_category = new TPaveText(0.6600, legend_y0 - 0.0550, 0.9350, legend_y0, "NDC");
else label_category = new TPaveText(0.2350, 0.8500, 0.5150, 0.9100, "NDC");
label_category->SetTextAlign(13);
label_category->AddText(label.data());
label_category->SetTextSize(0.055);
label_category->SetTextColor(1);
label_category->SetFillStyle(0);
label_category->SetBorderSize(0);
label_category->Draw();
canvas->cd();
bottomPad->Draw();
bottomPad->cd();
TH1* histogramRatio = (TH1*)histogram_data_density->Clone("histogramRatio");
if ( !histogramRatio->GetSumw2N() ) histogramRatio->Sumw2();
histogramRatio->SetTitle("");
histogramRatio->SetStats(false);
histogramRatio->SetMinimum(-0.99);
histogramRatio->SetMaximum(+0.99);
histogramRatio->SetMarkerColor(histogram_data_density->GetMarkerColor());
histogramRatio->SetMarkerStyle(histogram_data_density->GetMarkerStyle());
histogramRatio->SetMarkerSize(histogram_data_density->GetMarkerSize());
histogramRatio->SetLineColor(histogram_data_density->GetLineColor());
TH1* histogramRatioUncertainty = (TH1*)histogram_data_density->Clone("histogramRatioUncertainty");
if ( !histogramRatioUncertainty->GetSumw2N() ) histogramRatioUncertainty->Sumw2();
histogramRatioUncertainty->SetMarkerColor(10);
histogramRatioUncertainty->SetMarkerSize(0);
setStyle_uncertainty(histogramRatioUncertainty);
int numBins_bottom = histogramRatio->GetNbinsX();
for ( int iBin = 1; iBin <= numBins_bottom; ++iBin ) {
double binContent_data = histogram_data_density->GetBinContent(iBin);
double binError_data = histogram_data_density->GetBinError(iBin);
double binContent_mc = 0;
double binError_mc = 0;
if ( histogramSum_mc && histogramErr_mc ) {
binContent_mc = histogramSum_mc_density->GetBinContent(iBin);
binError_mc = histogramErr_mc_density->GetBinError(iBin);
} else {
TList* histograms = histogramStack_mc->GetHists();
TIter nextHistogram(histograms);
double binError2_mc = 0.;
while ( TH1* histogram_density = dynamic_cast<TH1*>(nextHistogram()) ) {
binContent_mc += histogram_density->GetBinContent(iBin);
binError2_mc += square(histogram_density->GetBinError(iBin));
}
binError_mc = TMath::Sqrt(binError2_mc);
}
if ( binContent_mc > 0. ) {
histogramRatio->SetBinContent(iBin, binContent_data/binContent_mc - 1.0);
histogramRatio->SetBinError(iBin, binError_data/binContent_mc);
histogramRatioUncertainty->SetBinContent(iBin, 0.);
histogramRatioUncertainty->SetBinError(iBin, binError_mc/binContent_mc);
}
}
std::cout << "histogramRatio = " << histogramRatio << std::endl;
dumpHistogram(histogramRatio);
std::cout << "histogramRatioUncertainty = " << histogramRatioUncertainty << std::endl;
dumpHistogram(histogramRatioUncertainty);
TAxis* xAxis_bottom = histogramRatio->GetXaxis();
assert(xAxis_bottom);
xAxis_bottom->SetTitle(xAxis_top->GetTitle());
xAxis_bottom->SetLabelColor(1);
xAxis_bottom->SetTitleColor(1);
xAxis_bottom->SetTitleOffset(1.05);
xAxis_bottom->SetTitleSize(0.16);
xAxis_bottom->SetTitleFont(xAxis_top->GetTitleFont());
xAxis_bottom->SetLabelOffset(0.02);
xAxis_bottom->SetLabelSize(0.12);
xAxis_bottom->SetTickLength(0.065);
xAxis_bottom->SetNdivisions(505);
TAxis* yAxis_bottom = histogramRatio->GetYaxis();
assert(yAxis_bottom);
yAxis_bottom->SetTitle("#frac{Data - Expectation}{Expectation}");
yAxis_bottom->SetLabelColor(1);
yAxis_bottom->SetTitleColor(1);
yAxis_bottom->SetTitleOffset(0.95);
yAxis_bottom->SetTitleFont(yAxis_top->GetTitleFont());
yAxis_bottom->SetNdivisions(505);
yAxis_bottom->CenterTitle();
yAxis_bottom->SetTitleSize(0.095);
yAxis_bottom->SetLabelSize(0.110);
yAxis_bottom->SetTickLength(0.04);
histogramRatio->Draw("axis");
TF1* line = new TF1("line","0", xAxis_bottom->GetXmin(), xAxis_bottom->GetXmax());
line->SetLineStyle(3);
line->SetLineWidth(1.5);
line->SetLineColor(kBlack);
line->Draw("same");
histogramRatioUncertainty->Draw("e2same");
if ( !doKeepBlinded ) {
histogramRatio->Draw("epsame");
}
histogramRatio->Draw("axissame");
canvas->Update();
size_t idx = outputFileName.find_last_of('.');
std::string outputFileName_plot = std::string(outputFileName, 0, idx);
if ( useLogScale ) outputFileName_plot.append("_log");
else outputFileName_plot.append("_linear");
canvas->Print(std::string(outputFileName_plot).append(".pdf").data());
canvas->Print(std::string(outputFileName_plot).append(".root").data());
//delete label_cms;
delete topPad;
delete label_category;
delete histogramRatio;
delete histogramRatioUncertainty;
delete line;
delete bottomPad;
delete canvas;
}
int main(){
square();
}
void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder)
#endif // AUTO_BED_LEVELING_FEATURE
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) idle();
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x, y);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS];
target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
float dx = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS];
// DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
if (marlin_debug_flags & DEBUG_DRYRUN)
position[E_AXIS] = target[E_AXIS];
float de = target[E_AXIS] - position[E_AXIS];
#if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
if (de) {
if (degHotend(extruder) < extrude_min_temp) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#if ENABLED(PREVENT_LENGTHY_EXTRUDE)
if (labs(de) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t* block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#if ENABLED(COREXY)
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps[A_AXIS] = labs(dx + dy);
block->steps[B_AXIS] = labs(dx - dy);
block->steps[Z_AXIS] = labs(dz);
#elif ENABLED(COREXZ)
// corexz planning
block->steps[A_AXIS] = labs(dx + dz);
block->steps[Y_AXIS] = labs(dy);
block->steps[C_AXIS] = labs(dx - dz);
#else
// default non-h-bot planning
block->steps[X_AXIS] = labs(dx);
block->steps[Y_AXIS] = labs(dy);
block->steps[Z_AXIS] = labs(dz);
#endif
block->steps[E_AXIS] = labs(de);
block->steps[E_AXIS] *= volumetric_multiplier[extruder];
block->steps[E_AXIS] *= extruder_multiplier[extruder];
block->steps[E_AXIS] /= 100;
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) return;
block->fan_speed = fanSpeed;
#if ENABLED(BARICUDA)
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
uint8_t db = 0;
#if ENABLED(COREXY)
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_HEAD); // ...and Y
if (dz < 0) db |= BIT(Z_AXIS);
if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
#elif ENABLED(COREXZ)
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_AXIS);
if (dz < 0) db |= BIT(Z_HEAD); // ...and Z
if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
#else
if (dx < 0) db |= BIT(X_AXIS);
if (dy < 0) db |= BIT(Y_AXIS);
if (dz < 0) db |= BIT(Z_AXIS);
#endif
if (de < 0) db |= BIT(E_AXIS);
block->direction_bits = db;
block->active_extruder = extruder;
//enable active axes
#if ENABLED(COREXY)
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
enable_x();
enable_y();
}
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#elif ENABLED(COREXZ)
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
enable_x();
enable_z();
}
if (block->steps[Y_AXIS]) enable_y();
#else
if (block->steps[X_AXIS]) enable_x();
if (block->steps[Y_AXIS]) enable_y();
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#endif
// Enable extruder(s)
if (block->steps[E_AXIS]) {
if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder
for (int i = 0; i < EXTRUDERS; i++)
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
switch(extruder) {
case 0:
enable_e0();
#if ENABLED(DUAL_X_CARRIAGE)
if (extruder_duplication_enabled) {
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2;
}
#endif
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2;
#if EXTRUDERS > 1
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
#endif
break;
#if EXTRUDERS > 1
case 1:
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
break;
#if EXTRUDERS > 2
case 2:
enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
break;
#if EXTRUDERS > 3
case 3:
enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
if (g_uc_extruder_last_move[2] == 0) disable_e2();
break;
#endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
}
}
else { // enable all
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
}
if (block->steps[E_AXIS])
NOLESS(feed_rate, minimumfeedrate);
else
NOLESS(feed_rate, mintravelfeedrate);
/**
* This part of the code calculates the total length of the movement.
* For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
* But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
* and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
* So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#if ENABLED(COREXY)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
#elif ENABLED(COREXZ)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
#else
float delta_mm[4];
delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
#endif
delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0;
if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else {
block->millimeters = sqrt(
#if ENABLED(COREXY)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
#elif ENABLED(COREXZ)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
#else
square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
#endif
);
}
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued = movesplanned();
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
bool mq = moves_queued > 1 && moves_queued < BLOCK_BUFFER_SIZE / 2;
#if ENABLED(OLD_SLOWDOWN)
if (mq) feed_rate *= 2.0 * moves_queued / BLOCK_BUFFER_SIZE;
#endif
#if ENABLED(SLOWDOWN)
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if (mq) {
if (segment_time < minsegmenttime) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0 / inverse_second);
#endif
}
}
#endif
#endif
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#if ENABLED(FILAMENT_SENSOR)
//FMM update ring buffer used for delay with filament measurements
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized
const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;
delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis
while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
while (delay_dist < 0) delay_dist += MMD10;
delay_index1 = delay_dist / 10.0; // calculate index
delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above)
if (delay_index1 != delay_index2) { // moved index
meas_sample = widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
while (delay_index1 != delay_index2) {
// Increment and loop around buffer
if (++delay_index2 >= MMD) delay_index2 -= MMD;
delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY);
measurement_delay[delay_index2] = meas_sample;
}
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS];
float speed_factor = 1.0; //factor <=1 do decrease speed
for (int i = 0; i < NUM_AXIS; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
float cs = fabs(current_speed[i]), mf = max_feedrate[i];
if (cs > mf) speed_factor = min(speed_factor, mf / cs);
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
long xs0 = axis_segment_time[X_AXIS][0],
xs1 = axis_segment_time[X_AXIS][1],
xs2 = axis_segment_time[X_AXIS][2],
ys0 = axis_segment_time[Y_AXIS][0],
ys1 = axis_segment_time[Y_AXIS][1],
ys2 = axis_segment_time[Y_AXIS][2];
if ((direction_change & BIT(X_AXIS)) != 0) {
xs2 = axis_segment_time[X_AXIS][2] = xs1;
xs1 = axis_segment_time[X_AXIS][1] = xs0;
xs0 = 0;
}
xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;
if ((direction_change & BIT(Y_AXIS)) != 0) {
ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
ys0 = 0;
}
ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
long max_x_segment_time = max(xs0, max(xs1, xs2)),
max_y_segment_time = max(ys0, max(ys1, ys2)),
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
if (min_xy_segment_time < MAX_FREQ_TIME) {
float low_sf = speed_factor * min_xy_segment_time / MAX_FREQ_TIME;
speed_factor = min(speed_factor, low_sf);
}
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed, block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#if ENABLED(ADVANCE)
// Calculate advance rate
if (!bse || (!bsx && !bsy && !bsz)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256;
block->advance = advance;
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
st_wake_up();
} // plan_buffer_line()
void events(void) {
int ictbeam=0, ipage=0, istract=0, line, i, j, k, l, ixhold, iyhold;
double fintim = d.date + Time, datemin, xtime, repair, yank;
#ifdef DEBUG
if (idebug) prout("EVENTS");
#endif
if (stdamtim == 1e30 && !REPORTS)
{
/* chart will no longer be updated because radio is dead */
stdamtim = d.date;
for (i=1; i <= 8 ; i++)
for (j=1; j <= 8; j++)
if (starch[i][j] == 1) starch[i][j] = d.galaxy[i][j]+1000;
}
for (;;) {
/* Select earliest extraneous event, line==0 if no events */
line = FSPY;
if (alldone) return;
datemin = fintim;
for (l=1; l<=NEVENTS; l++)
if (future[l] <= datemin) {
line = l;
datemin = future[l];
}
xtime = datemin-d.date;
#ifdef CLOAKING
if (iscloaking) {
energy -= xtime*500.0;
if (energy <= 0.) {
finish(FNRG);
return;
}
}
#endif
d.date = datemin;
/* Decrement Federation resources and recompute remaining time */
d.remres -= (d.remkl+4*d.remcom)*xtime;
d.remtime = d.remres/(d.remkl+4*d.remcom);
if (d.remtime <=0) {
finish(FDEPLETE);
return;
}
/* Is life support adequate? */
if (damage[DLIFSUP] && condit != IHDOCKED) {
if (lsupres < xtime && damage[DLIFSUP] > lsupres) {
finish(FLIFESUP);
return;
}
lsupres -= xtime;
if (damage[DLIFSUP] <= xtime) lsupres = inlsr;
}
/* Fix devices */
repair = xtime;
if (condit == IHDOCKED) repair /= docfac;
/* Don't fix Deathray here */
for (l=1; l<=ndevice; l++)
if (damage[l] > 0.0 && l != DDRAY)
damage[l] -= (damage[l]-repair > 0.0 ? repair : damage[l]);
/* Fix Deathray if docked */
if (damage[DDRAY] > 0.0 && condit == IHDOCKED)
damage[DDRAY] -= (damage[l] - xtime > 0.0 ? xtime : damage[DDRAY]);
/* If radio repaired, update star chart and attack reports */
if (stdamtim != 1e30 && REPORTS) {
stdamtim = 1e30;
prout("Lt. Uhura- \"Captain, the sub-space radio is working and");
prout(" surveillance reports are coming in.");
skip(1);
for (i=1; i <= 8 ; i++)
for (j=1; j <= 8; j++)
if (starch[i][j] > 999) starch[i][j] = 1;
if (iseenit==0) {
attakreport();
iseenit = 1;
}
skip(1);
prout(" The star chart is now up to date.\"");
skip(1);
}
/* Cause extraneous event LINE to occur */
Time -= xtime;
switch (line) {
case FSNOVA: /* Supernova */
if (ipage==0) pause(1);
ipage=1;
snova(0,0);
future[FSNOVA] = d.date + expran(0.5*intime);
if (d.galaxy[quadx][quady] == 1000) return;
break;
case FSPY: /* Check with spy to see if S.C. should tractor beam */
if (d.nscrem == 0 ||
#ifdef CLOAKING
iscloaked || /* Cannot tractor beam if we can't be seen! */
#endif
ictbeam+istract > 0 ||
condit==IHDOCKED || isatb==1 || iscate==1) return;
if (ientesc ||
(energy < 2000 && torps < 4 && shield < 1250) ||
(damage[DPHASER]>0 && (damage[DPHOTON]>0 || torps < 4)) ||
(damage[DSHIELD] > 0 &&
(energy < 2500 || damage[DPHASER] > 0) &&
(torps < 5 || damage[DPHOTON] > 0))) {
/* Tractor-beam her! */
istract=1;
yank = square(d.isx-quadx) + square(d.isy-quady);
/*********TBEAM CODE***********/
}
else return;
case FTBEAM: /* Tractor beam */
if (line==FTBEAM) {
if (d.remcom == 0) {
future[FTBEAM] = 1e30;
break;
}
i = Rand()*d.remcom+1.0;
yank = square(d.cx[i]-quadx) + square(d.cy[i]-quady);
if (istract || condit == IHDOCKED ||
#ifdef CLOAKING
iscloaked || /* cannot tractor beam if we can't be seen */
#endif
yank == 0) {
/* Drats! Have to reschedule */
future[FTBEAM] = d.date + Time +
expran(1.5*intime/d.remcom);
break;
}
}
/* tractor beaming cases merge here */
yank = sqrt(yank);
if (ipage==0) pause(1);
ipage=1;
Time = (10.0/(7.5*7.5))*yank; /* 7.5 is yank rate (warp 7.5) */
ictbeam = 1;
skip(1);
proutn("***");
crmshp();
prout(" caught in long range tractor beam--");
/* If Kirk & Co. screwing around on planet, handle */
atover(1); /* atover(1) is Grab */
if (alldone) return;
if (icraft == 1) { /* Caught in Galileo? */
finish(FSTRACTOR);
return;
}
/* Check to see if shuttle is aboard */
if (iscraft==0) {
skip(1);
if (Rand() >0.5) {
prout("Galileo, left on the planet surface, is captured");
prout("by aliens and made into a flying McDonald's.");
damage[DSHUTTL] = -10;
iscraft = -1;
}
else {
prout("Galileo, left on the planet surface, is well hidden.");
}
}
if (line==0) {
quadx = d.isx;
quady = d.isy;
}
else {
quadx = d.cx[i];
quady = d.cy[i];
}
iran10(§x, §y);
crmshp();
proutn(" is pulled to");
cramlc(1, quadx, quady);
proutn(", ");
cramlc(2, sectx, secty);
skip(1);
if (resting) {
prout("(Remainder of rest/repair period cancelled.)");
resting = 0;
}
if (shldup==0) {
if (damage[DSHIELD]==0 && shield > 0) {
sheild(2); /* Shldsup */
shldchg=0;
}
else prout("(Shields not currently useable.)");
}
newqad(0);
/* Adjust finish time to time of tractor beaming */
fintim = d.date+Time;
if (d.remcom <= 0) future[FTBEAM] = 1e30;
else future[FTBEAM] = d.date+Time+expran(1.5*intime/d.remcom);
break;
case FSNAP: /* Snapshot of the universe (for time warp) */
snapsht = d;
d.snap = 1;
future[FSNAP] = d.date + expran(0.5 * intime);
break;
case FBATTAK: /* Commander attacks starbase */
if (d.remcom==0 || d.rembase==0) {
/* no can do */
future[FBATTAK] = future[FCDBAS] = 1e30;
break;
}
i = 0;
for (j=1; j<=d.rembase; j++) {
for (k=1; k<=d.remcom; k++)
if (d.baseqx[j]==d.cx[k] && d.baseqy[j]==d.cy[k] &&
(d.baseqx[j]!=quadx || d.baseqy[j]!=quady) &&
(d.baseqx[j]!=d.isx || d.baseqy[j]!=d.isy)) {
i = 1;
break;
}
if (i == 1) break;
}
if (j>d.rembase) {
/* no match found -- try later */
future[FBATTAK] = d.date + expran(0.3*intime);
future[FCDBAS] = 1e30;
break;
}
/* commander + starbase combination found -- launch attack */
batx = d.baseqx[j];
baty = d.baseqy[j];
future[FCDBAS] = d.date+1.0+3.0*Rand();
if (isatb) /* extra time if SC already attacking */
future[FCDBAS] += future[FSCDBAS]-d.date;
future[FBATTAK] = future[FCDBAS] +expran(0.3*intime);
iseenit = 0;
if (!REPORTS)
break; /* No warning :-( */
iseenit = 1;
if (ipage==0) pause(1);
ipage = 1;
skip(1);
proutn("Lt. Uhura- \"Captain, the starbase in");
cramlc(1, batx, baty);
skip(1);
prout(" reports that it is under atttack and that it can");
proutn(" hold out only until stardate ");
cramf(future[FCDBAS],1,1);
prout(".\"");
if (resting) {
skip(1);
proutn("Mr. Spock- \"Captain, shall we cancel the rest period?\"");
if (ja()) {
resting = 0;
Time = 0.0;
return;
}
}
break;
case FSCDBAS: /* Supercommander destroys base */
future[FSCDBAS] = 1e30;
isatb = 2;
if (d.galaxy[d.isx][d.isy]%100 < 10) break; /* WAS RETURN! */
ixhold = batx;
iyhold = baty;
batx = d.isx;
baty = d.isy;
case FCDBAS: /* Commander succeeds in destroying base */
if (line==FCDBAS) {
future[FCDBAS] = 1e30;
/* find the lucky pair */
for (i = 1; i <= d.remcom; i++)
if (d.cx[i]==batx && d.cy[i]==baty) break;
if (i > d.remcom || d.rembase == 0 ||
d.galaxy[batx][baty] % 100 < 10) {
/* No action to take after all */
batx = baty = 0;
break;
}
}
/* Code merges here for any commander destroying base */
/* Not perfect, but will have to do */
if (starch[batx][baty] == -1) starch[batx][baty] = 0;
/* Handle case where base is in same quadrant as starship */
if (batx==quadx && baty==quady) {
if (starch[batx][baty] > 999) starch[batx][baty] -= 10;
quad[basex][basey]= IHDOT;
basex=basey=0;
newcnd();
skip(1);
prout("Spock- \"Captain, I believe the starbase has been destroyed.\"");
}
else if (d.rembase != 1 && REPORTS) {
/* Get word via subspace radio */
if (ipage==0) pause(1);
ipage = 1;
skip(1);
prout("Lt. Uhura- \"Captain, Starfleet Command reports that");
proutn(" the starbase in");
cramlc(1, batx, baty);
prout(" has been destroyed by");
if (isatb==2) prout("the Klingon Super-Commander");
else prout("a Klingon Commander");
}
/* Remove Starbase from galaxy */
d.galaxy[batx][baty] -= 10;
for (i=1; i <= d.rembase; i++)
if (d.baseqx[i]==batx && d.baseqy[i]==baty) {
d.baseqx[i]=d.baseqx[d.rembase];
d.baseqy[i]=d.baseqy[d.rembase];
}
d.rembase--;
if (isatb == 2) {
/* reinstate a commander's base attack */
batx = ixhold;
baty = iyhold;
isatb = 0;
}
else {
batx = baty = 0;
}
break;
case FSCMOVE: /* Supercommander moves */
future[FSCMOVE] = d.date+0.2777;
if (ientesc+istract==0 &&
isatb!=1 &&
(iscate!=1 || justin==1)) scom(&ipage);
break;
case FDSPROB: /* Move deep space probe */
future[FDSPROB] = d.date + 0.01;
probex += probeinx;
probey += probeiny;
i = (int)(probex/10 +0.05);
j = (int)(probey/10 + 0.05);
if (probecx != i || probecy != j) {
probecx = i;
probecy = j;
if (i < 1 || i > 8 || j < 1 || j > 8 ||
d.galaxy[probecx][probecy] == 1000) {
// Left galaxy or ran into supernova
if (REPORTS) {
if (ipage==0) pause(1);
ipage = 1;
skip(1);
proutn("Lt. Uhura- \"The deep space probe ");
if (i < 1 ||i > 8 || j < 1 || j > 8)
proutn("has left the galaxy");
else
proutn("is no longer transmitting");
prout(".\"");
}
future[FDSPROB] = 1e30;
break;
}
if (REPORTS) {
if (ipage==0) pause(1);
ipage = 1;
skip(1);
proutn("Lt. Uhura- \"The deep space probe is now in ");
cramlc(1, probecx, probecy);
prout(".\"");
}
}
/* Update star chart if Radio is working or have access to
radio. */
if (REPORTS)
starch[probecx][probecy] = damage[DRADIO] > 0.0 ?
d.galaxy[probecx][probecy]+1000 : 1;
proben--; // One less to travel
if (proben == 0 && isarmed &&
d.galaxy[probecx][probecy] % 10 > 0) {
/* lets blow the sucker! */
snova(1,0);
future[FDSPROB] = 1e30;
if (d.galaxy[quadx][quady] == 1000) return;
}
break;
}
}
}
