static int
separation(double const dist,
double const eyesep,
unsigned int const dpi,
double const dof /* depth of field */
) {
/*----------------------------------------------------------------------------
Return a separation in pixels which corresponds to a 3-D distance
between the viewer's eyes and a point on an object.
-----------------------------------------------------------------------------*/
int const pixelEyesep = round2int(eyesep * dpi);
return round2int((1.0 - dof * dist) * pixelEyesep / (2.0 - dof * dist));
}
void outputimgyuv(char *fname,unsigned char *img,int ny,int nx)
{
FILE *fimg;
int iy,ix,i,j,imagesize,loc1,loc2,loc3,loc0;
unsigned char *tmp;
rgb2yuv(img,ny*nx*3);
imagesize = ny*nx;
tmp=(unsigned char *)malloc(imagesize*sizeof(unsigned char));
fimg=fopen(fname,"wb");
for (i=0,j=0;i<imagesize;i++,j+=3) tmp[i] = img[j];
fwrite(tmp, sizeof(unsigned char), imagesize, fimg);
i=0;
for (iy=0;iy<ny;iy+=2)
{
for (ix=0;ix<nx;ix+=2)
{
loc0 = LOC(iy,ix,1,nx,3);
loc1 = loc0+3;
loc2 = loc0+3*nx;
loc3 = loc2+3;
tmp[i] = (unsigned char) round2int(( ((float) img[loc0]) +
img[loc1]+img[loc2]+img[loc3]) /4);
i++;
}
}
fwrite(tmp, sizeof(unsigned char), imagesize/4, fimg);
i=0;
for (iy=0;iy<ny;iy+=2)
{
for (ix=0;ix<nx;ix+=2)
{
loc0 = LOC(iy,ix,2,nx,3);
loc1 = loc0+3;
loc2 = loc0+3*nx;
loc3 = loc2+3;
tmp[i] = (unsigned char) round2int(( ((float) img[loc0]) +
img[loc1]+img[loc2]+img[loc3]) /4);
i++;
}
}
fwrite(tmp, sizeof(unsigned char), imagesize/4, fimg);
free(tmp);
fclose (fimg);
}
/* Do the bulk of the work. See the paper cited above for code
* comments. All I (Scott) did was transcribe the code and make
* minimal changes for Netpbm. And some style changes by Bryan to
* match Netpbm style.
*/
static void
makeStereoRow(const struct pam * const inPamP,
tuple * const inRow,
int * const same,
double const depthOfField,
double const eyesep,
unsigned int const dpi) {
#define Z(X) (1.0-inRow[X][0]/(double)inPamP->maxval)
int const width = inPamP->width;
int const pixelEyesep = round2int(eyesep * dpi);
/* Separation in pixels between the viewer's eyes */
int col;
for (col = 0; col < width; ++col)
same[col] = col;
for (col = 0; col < width; ++col) {
int const s = separation(Z(col), eyesep, dpi, depthOfField);
int left, right;
left = col - s/2; /* initial value */
right = left + s; /* initial value */
if (0 <= left && right < width) {
int visible;
int t;
double zt;
t = 1; /* initial value */
do {
double const dof = depthOfField;
zt = Z(col) + 2.0*(2.0 - dof*Z(col))*t/(dof*pixelEyesep);
visible = Z(col-t) < zt && Z(col+t) < zt;
++t;
} while (visible && zt < 1);
if (visible) {
int l;
l = same[left];
while (l != left && l != right) {
if (l < right) {
left = l;
l = same[left];
} else {
same[left] = right;
left = right;
l = same[left];
right = l;
}
}
same[left] = right;
}
}
}
}
void setfn1(struct data *d1,struct data *d2,double multiplier)
{
int fn1;
if (d2->fn1==0) fn1=d2->nv*2;
else fn1=d2->fn1;
fn1=round2int(multiplier*fn1);
setval(&d1->p,"fn1",fn1);
d1->fn1=fn1;
}
void setfn(struct data *d1,struct data *d2,double multiplier)
{
int fn;
if (d2->fn==0) fn=d2->np;
else fn=d2->fn;
fn=round2int(multiplier*fn);
setval(&d1->p,"fn",fn);
d1->fn=fn;
}
void operator()(T *outPtr, const T *inPtr, const af::dim4 &odims, const af::dim4 &idims,
const af::dim4 &ostrides, const af::dim4 &istrides,
const dim_type x, const dim_type y)
{
// Compute Indices
dim_type i_x = round2int((float)x / (odims[0] / (float)idims[0]));
dim_type i_y = round2int((float)y / (odims[1] / (float)idims[1]));
if (i_x >= idims[0]) i_x = idims[0] - 1;
if (i_y >= idims[1]) i_y = idims[1] - 1;
dim_type i_off = i_y * istrides[1] + i_x;
dim_type o_off = y * ostrides[1] + x;
// Copy values from all channels
for(dim_type z = 0; z < odims[2]; z++) {
outPtr[o_off + z * ostrides[2]] = inPtr[i_off + z * istrides[2]];
}
}
void outfloat2raw(char *fname,float *img,int ny,int nx,int dim)
{
FILE *fimg;
int i,j,imagesize;
unsigned char *tmp;
imagesize = ny*nx*dim;
tmp=(unsigned char *)malloc(imagesize*sizeof(unsigned char));
for (i=0;i<imagesize;i++)
{
if (img[i]>255) img[i]=255;
if (img[i]<0) img[i]=0;
tmp[i]=(unsigned char) round2int(img[i]);
}
fimg=fopen(fname,"wb");
fwrite(tmp, sizeof(unsigned char), imagesize, fimg);
fclose(fimg);
free(tmp);
}
static void
reportParameters(struct cmdlineInfo const cmdline) {
unsigned int const pixelEyesep = round2int(cmdline.eyesep * cmdline.dpi);
pm_message("Eye separation: %.4g inch * %d DPI = %u pixels",
cmdline.eyesep, cmdline.dpi, pixelEyesep);
if (cmdline.magnifypat > 1)
pm_message("Background magnification: %uX * %uX",
cmdline.magnifypat, cmdline.magnifypat);
pm_message("\"Optimal\" pattern width: %u / (%u * 2) = %u pixels",
pixelEyesep, cmdline.magnifypat,
pixelEyesep/(cmdline.magnifypat * 2));
pm_message("Unique 3-D depth levels possible: %u",
separation(0, cmdline.eyesep, cmdline.dpi, cmdline.depth) -
separation(1, cmdline.eyesep, cmdline.dpi, cmdline.depth) + 1);
if (cmdline.patFilespec && (cmdline.xshift || cmdline.yshift))
pm_message("Pattern shift: (%u, %u)", cmdline.xshift, cmdline.yshift);
}
void track :: synth(SampleBuf *out,
long writePos,
int c,
long synthtime,
int steps,
real fScale0,
real fScale1,
real mScale) {
if(point.size()==0) return;
long k = synthtime - start;
if(k>=(long)point.size()-1) return;
currtime = synthtime;
if(k<0) return;
int k1 = k + 1;
tpoint *tp0 = point[k];
tpoint *tp1 = point[k1];
real w0 = tp0->f;
real w1 = tp1->f;
real ph0 = tp0->ph;
real ph1 = tp1->ph;
real h = tp0->h;
real dp = ph1 - ph0;
if(dp>PI) dp-=TWOPI;
else if(dp<-PI) dp+=TWOPI;
real dp0 = 0.5f*h*(w0 + w1);
real dw = canon(dp - dp0)/h;
if(k==0) {
if(precursor) {
m_p = precursor->m_pDescendant;
} else {
dw = 0;
}
}
real dt = (real)steps;
w0 = (w0+dw);
w1 = (w1+dw);
w0 = w0*fScale0;
w1 = w1*fScale1;
dp = dt*0.5f*(w0 + w1);
real b = (w1 - w0)/(2.0f*dt);
bool bEnd = (k1==(long)point.size()-1);
bool bStart = (k==0);
if(bStart && tailStart) {
if(w0 < PI && w0 > -PI) {
real ph = m_p;
int rise = round2int(this->rise * (real)steps);
real dm = mScale*tp1->y/(real)rise;
real m = mScale*tp1->y - dm;
for(int i=steps-1;i>=steps-rise+1;i--) {
ph -= w0;
if(ph<-PI) ph += TWOPI;
else if(ph>PI) ph -= TWOPI;
out->buf[writePos+i][c] += m * COS(ph);
m -= dm;
}
}
} else if(bEnd && tailEnd) {
if(w0 < PI && w0 > -PI) {
real ph = m_p;
int fall = round2int(this->fall * (real)steps);
real dm = mScale*tp0->y/(real)fall;
real m = mScale*tp0->y;
for(int i=0;i<fall;i++) {
out->buf[writePos+i][c] += m * COS(ph);
ph += w0;
if(ph<-PI) ph += TWOPI;
else if(ph>PI) ph -= TWOPI;
m -= dm;
}
}
} else {
real m = mScale*tp0->y;
real dm = mScale*(tp1->y0 - tp0->y)/dt;
real ph = m_p;
real b2tt1 = b;
real b2 = 2.0f*b;
real dph;
audio *o = &(out->buf[writePos]);
for(int i=0;i<steps;i++) {
dph = w0 + b2tt1;
if(dph < PI && dph > -PI) (*o)[c] += m * COS(ph);
ph += dph;
if(ph<-PI) ph += TWOPI;
else if(ph>PI) ph -= TWOPI;
b2tt1 += b2;
m += dm;
o++;
}
}
if(bEnd) {
if(descendant && descendant->back()->M < tp0->M) {
m_pDescendant = canon(m_p + dp/dt*(real)(descendant->owner->samplePos - owner->samplePos));
}
} else if(bStart && tailStart) {
} else {
m_p = canon(m_p + dp);
if(descendant && descendant->back()->M > tp0->M && (k1+res==(long)point.size()-1)) {
m_pDescendant = canon(m_p + dp/dt*(real)(descendant->owner->samplePos - (owner->samplePos+steps)));
}
}
}
static const char *special_commands(void *userdata, char letter, float value,
const char *remaining) {
GCodeMachineControl_t *state = (GCodeMachineControl_t*)userdata;
const int code = (int)value;
if (letter == 'M') {
int pin = -1;
int aux_bit = -1;
switch (code) {
case 0: set_gpio(ESTOP_SW_GPIO); break;
case 3:
case 4:
for (;;) {
const char* after_pair = gcodep_parse_pair(remaining, &letter, &value,
state->msg_stream);
if (after_pair == NULL) break;
else if (letter == 'S') state->spindle_rpm = round2int(value);
else break;
remaining = after_pair;
}
if (state->spindle_rpm) {
state->aux_bits |= AUX_BIT_SPINDLE_ON;
if (code == 3) state->aux_bits &= ~AUX_BIT_SPINDLE_DIR;
else state->aux_bits |= AUX_BIT_SPINDLE_DIR;
}
break;
case 5: state->aux_bits &= ~(AUX_BIT_SPINDLE_ON | AUX_BIT_SPINDLE_DIR); break;
case 7: state->aux_bits |= AUX_BIT_MIST; break;
case 8: state->aux_bits |= AUX_BIT_FLOOD; break;
case 9: state->aux_bits &= ~(AUX_BIT_MIST | AUX_BIT_FLOOD); break;
case 10: state->aux_bits |= AUX_BIT_VACUUM; break;
case 11: state->aux_bits &= ~AUX_BIT_VACUUM; break;
case 42:
case 62:
case 63:
case 64:
case 65:
for (;;) {
const char* after_pair = gcodep_parse_pair(remaining, &letter, &value,
state->msg_stream);
if (after_pair == NULL) break;
if (letter == 'P') pin = round2int(value);
else if (letter == 'S' && code == 42) aux_bit = round2int(value);
else break;
remaining = after_pair;
}
if (code == 62 || code == 64)
aux_bit = 1;
else if (code == 63 || code == 65)
aux_bit = 0;
if (pin >= 0 && pin <= MAX_AUX_PIN) {
if (aux_bit >= 0 && aux_bit <= 1) {
if (aux_bit) state->aux_bits |= 1 << pin;
else state->aux_bits &= ~(1 << pin);
} else if (code == 42 && state->msg_stream) { // Just read operation.
mprintf(state, "%d\n", (state->aux_bits >> pin) & 1);
}
}
break;
case 80: set_gpio(MACHINE_PWR_GPIO); break;
case 81: clr_gpio(MACHINE_PWR_GPIO); break;
case 105: mprintf(state, "T-300\n"); break; // no temp yet.
case 114:
if (buffer_available(&state->buffer)) {
struct AxisTarget *current = buffer_at(&state->buffer, 0);
const int *mpos = current->position_steps;
const float x = 1.0f * mpos[AXIS_X] / state->cfg.steps_per_mm[AXIS_X];
const float y = 1.0f * mpos[AXIS_Y] / state->cfg.steps_per_mm[AXIS_Y];
const float z = 1.0f * mpos[AXIS_Z] / state->cfg.steps_per_mm[AXIS_Z];
const float e = 1.0f * mpos[AXIS_E] / state->cfg.steps_per_mm[AXIS_E];
const float *origin = state->coordinate_display_origin;
mprintf(state, "X:%.3f Y:%.3f Z:%.3f E:%.3f",
x - origin[AXIS_X], y - origin[AXIS_Y], z - origin[AXIS_Z],
e - origin[AXIS_E]);
mprintf(state, " [ABS. MACHINE CUBE X:%.3f Y:%.3f Z:%.3f]", x, y, z);
switch (state->homing_state) {
case HOMING_STATE_NEVER_HOMED:
mprintf(state, " (Unsure: machine never homed!)\n");
break;
case HOMING_STATE_HOMED_BUT_MOTORS_UNPOWERED:
mprintf(state, " (Lower confidence: motor power off at "
"least once after homing)\n");
break;
case HOMING_STATE_HOMED:
mprintf(state, " (confident: machine was homed)\n");
break;
}
} else {
mprintf(state, "// no current pos\n");
}
break;
case 115: mprintf(state, "%s\n", VERSION_STRING); break;
case 117:
mprintf(state, "// Msg: %s\n", remaining); // TODO: different output ?
remaining = NULL; // consume the full line.
break;
case 119: {
char any_enstops_found = 0;
for (int axis = 0; axis < GCODE_NUM_AXES; ++axis) {
struct EndstopConfig config = state->min_endstop[axis];
if (config.endstop_number) {
int value = get_gpio(get_endstop_gpio_descriptor(config));
mprintf(state, "%c_min:%s ",
tolower(gcodep_axis2letter(axis)),
value == config.trigger_value ? "TRIGGERED" : "open");
any_enstops_found = 1;
}
config = state->max_endstop[axis];
if (config.endstop_number) {
int value = get_gpio(get_endstop_gpio_descriptor(config));
mprintf(state, "%c_max:%s ",
tolower(gcodep_axis2letter(axis)),
value == config.trigger_value ? "TRIGGERED" : "open");
any_enstops_found = 1;
}
}
if (any_enstops_found) {
mprintf(state, "\n");
} else {
mprintf(state, "// This machine has no endstops configured.\n");
}
}
break;
case 999: clr_gpio(ESTOP_SW_GPIO); break;
default:
mprintf(state, "// BeagleG: didn't understand ('%c', %d, '%s')\n",
letter, code, remaining);
remaining = NULL; // In this case, let's just discard remainig block.
break;
}
long Resampler :: read(audio *audioOut, long samples)
{
if(!bPull) {
frame.in = in->getReadBuf();
frame.size = in->n_readable();
if(frame.size) bInput = true;
}
long nRead = out->n_readable();
while(nRead < samples && bInput) {
if(bInput && inOffset == frame.size) {
if(!bPull) {
in->advance(frame.size);
bInput = false;
} else {
cb(data,&frame);
if(!frame.size)
bWritingComplete = true;
}
if(bWritingComplete) {
bInput = false;
out->grow(midAbs - writePosAbs);
out->writePos += midAbs - writePosAbs;
out->delay = 0;
}
inOffset = 0;
}
if(frame.size) {
real dratio = 1.0f/(real)frame.size*(frame.ratio1-frame.ratio0);
real ratio = frame.ratio0 + (real)inOffset*dratio;
real ratiorec = ratio<1.0f?1.0f:1.0f/ratio;
real f = ratiorec*SBSMS_SINC_RES;
int fi = round2int(f-0.5f);
real ff = f - fi;
if(ff<0.0f) {
ff += 1.0f;
fi--;
}
real scale = ratio<1.0f?ratio:1.0f;
int maxDist = round2int(sincZeros*ratio-0.5f);
// absolute start position
startAbs = max((long)0,midAbs-maxDist);
// samples to advance
long advance = max((long)0,startAbs - maxDist - writePosAbs);
writePosAbs += advance;
// absolute end position
endAbs = midAbs + maxDist;
// starting position in output
int start = startAbs - writePosAbs;
assert(start>=0);
// zero point in output
int mid = midAbs - writePosAbs;
// ending position in output
int end = endAbs - writePosAbs;
// provide extra delay for variable rate ratio
out->delay = maxDist<<1;
out->writePos += advance;
if(fabs(ratio-1.0f) < 1e-6f && fabs(dratio) < 1e-8f ) {
// how far ahead to write
int nAhead = mid+frame.size;
out->N = nAhead;
out->grow(nAhead);
long nWrite = min(SBSMS_RESAMPLE_CHUNK_SIZE,frame.size-inOffset);
for(int j=0;j<nWrite;j++) {
out->buf[out->writePos+mid+j][0] += frame.in[j+inOffset][0];
out->buf[out->writePos+mid+j][1] += frame.in[j+inOffset][1];
}
inOffset += nWrite;
midAbsf += ratio*nWrite;
int nWritten = round2int(midAbsf);
midAbsf -= nWritten;
midAbs += nWritten;
} else {
long nWrite = min(SBSMS_RESAMPLE_CHUNK_SIZE,frame.size-inOffset);
audio *i = &(frame.in[inOffset]);
for(int j=0;j<nWrite;j++) {
// how far ahead to write
int nAhead = end;
out->N = nAhead;
out->grow(nAhead);
audio *o = &(out->buf[out->writePos+start]);
real d = (start-mid-midAbsf)*f;
int di = round2int(d-0.5f);
real df = d-di;
if(df<0.0f) {
df += 1.0f;
di--;
}
real i0 = (*i)[0];
real i1 = (*i)[1];
for(int k=start;k<end;k++) {
int k0 = (di<0)?-di:di;
int k1 = (di<0)?k0-1:k0+1;
real sinc;
if(k1>=SBSMS_SINC_SIZE) {
if(k0>=SBSMS_SINC_SIZE) {
sinc = 0.0f;
} else {
sinc = scale*sincTable[k0];
}
} else if(k0>=SBSMS_SINC_SIZE) {
sinc = scale*sincTable[k1];
} else {
sinc = scale*((1.0f-df)*sincTable[k0] + df*sincTable[k1]);
}
(*o)[0] += i0 * sinc;
(*o)[1] += i1 * sinc;
di += fi;
df += ff;
if(!(df<1.0f)) {
df -= 1.0f;
di++;
}
o++;
}
i++;
midAbsf += ratio;
if(dratio != 0.0f) {
ratio += dratio;
ratiorec = ratio<1.0f?1.0f:1.0f/ratio;
f = ratiorec*SBSMS_SINC_RES;
fi = round2int(f-0.5f);
ff = f - fi;
if(ff<0.0f) {
ff += 1.0f;
fi--;
}
scale = ratio<1.0f?ratio:1.0f;
maxDist = round2int(sincZeros*ratio-0.5f);
}
int nWritten = round2int(midAbsf);
midAbsf -= nWritten;
midAbs += nWritten;
startAbs = max((long)0,midAbs-maxDist);
endAbs = midAbs + maxDist;
start = startAbs - writePosAbs;
mid = midAbs - writePosAbs;
end = endAbs - writePosAbs;
}
inOffset += nWrite;
}
}
nRead = out->n_readable();
}
nRead = min(samples,out->n_readable());
if(nRead) {
out->read(audioOut,nRead);
out->advance(nRead);
}
return nRead;
}
long Resampler :: samplesInOutput()
{
long samplesFromBuffer = round2int(0.5f*(frame.ratio0+frame.ratio1)*(frame.size-inOffset));
return out->writePos + midAbs - writePosAbs - out->readPos + samplesFromBuffer;
}
