/*
* 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;
}
