/* * Computes distances between a set of input points and * a given point. The distances are returned in a sorted array. */ void dsdist_s(int n, DSpoints3 p[], DSpoints3 q, float *sdist) /* * n - The number of points in the p array. * p - An array of n points. * q - An individual point. * sdist - A pointer to an array of n distances to point q. */ { DSpoints3 dp; int i; float ftmp; for (i = 0; i < n; i++) { dp.x = p[i].x - q.x; dp.y = p[i].y - q.y; dp.z = p[i].z - q.z; ftmp = mags(dp); if (ftmp <= (float) ds_max_dist * ds_scale) { sdist[i] = ftmp; } else { sdist[i] = -1.; } } }
StepperMotor::StepperMotor(): quarterTrack(QTRACKS >> 1), // start in the middle of the disk... just for fun // TODO if we want to be extremely accurate, we should save each arm's position on shutdown and restore on startup // (because in the real-life Apple ][, the arm stays in the same position when powered off). pos(0), mags(0) { } StepperMotor::~StepperMotor() { } signed char StepperMotor::mapMagPos[] = {-1,0,2,1,4,-1,3,2,6,7,-1,0,5,6,4,-1}; void StepperMotor::setMagnet(const unsigned char magnet, const bool on) { const unsigned char mask = 1 << magnet; if (on) { this->mags |= mask; } else { this->mags &= ~mask; } const char newPos = mapMagPos[this->mags]; char d; if (newPos >= 0) { d = calcDeltaPos(this->pos,newPos); this->pos = newPos; this->quarterTrack += d; if (this->quarterTrack < 0) this->quarterTrack = 0; else if (this->quarterTrack > QTRACKS) this->quarterTrack = QTRACKS; } /* std::cout << " ARM: magnet " << (unsigned int)magnet << " " << (on ? "on " : "off" ); std::cout << " [" << ((mags&1)?"*":".") << ((mags&2)?"*":".") << ((mags&4)?"*":".") << ((mags&8)?"*":".") << "]"; if (d != 0) { std::cout << " track " << std::hex << (unsigned int)(this->quarterTrack >> 2); int fract = this->quarterTrack & 3; if (fract != 0) { std::cout << (fract == 1 ? " +.25" : fract == 2 ? " +.5" : " +.75"); } } std::cout << std::endl; */ }
/* * Given three points in three space a, b, c, find the angle * between the vector from b to a and the vector from b to c. */ float dsangs(DSpoints3 a, DSpoints3 b, DSpoints3 c) { DSpoints3 vector_1,vector_2; float cosd; vector_1.x = a.x - b.x; vector_1.y = a.y - b.y; vector_1.z = a.z - b.z; vector_2.x = c.x - b.x; vector_2.y = c.y - b.y; vector_2.z = c.z - b.z; cosd = (float) dot_s(vector_1,vector_2)/(mags(vector_1)*mags(vector_2)); if (cosd > 1.) cosd = 1.; if (cosd < -1.) cosd = -1.; return((float) acos((double) cosd)); }
//returns a matrix of magnitudes given a matrix of x and y values Mat matrixMagnitude(Mat &matX, Mat &matY) { Mat mags(matX.rows,matX.cols,CV_32F); for (int y = 0; y < matX.rows; ++y) { const int *grad_x = matX.ptr<int>(y), *grad_y = matY.ptr<int>(y); float *Mr = mags.ptr<float>(y); for (int x = 0; x < matX.cols; ++x) { int gX = grad_x[x], gY = grad_y[x]; float magnitude = sqrt((gX * gX) + (gY * gY)); Mr[x] = magnitude; } } return mags; }
cv::Mat matrixMagnitude(const cv::Mat &matX, const cv::Mat &matY) { cv::Mat mags(matX.rows,matX.cols,CV_64F); for (int y = 0; y < matX.rows; ++y) { const double *Xr = matX.ptr<double>(y), *Yr = matY.ptr<double>(y); double *Mr = mags.ptr<double>(y); for (int x = 0; x < matX.cols; ++x) { double gX = Xr[x], gY = Yr[x]; double magnitude = sqrt((gX * gX) + (gY * gY)); Mr[x] = magnitude; } } return mags; }