double DockingProbe::CollisionDetection(VECTOR3 prbP, VECTOR3 prbD, VECTOR3 drgV, VECTOR3 drgA)
{
//This Function will check to see if collision has occured and
//return a vector of resultant forces if collision has occured
//If returns 0's, no collision occured.
double t = 0.0;
VECTOR3 Del;
double c0,c1,c2;
MATRIX3 M;
M = mul(drgA, drgA) - eyemat(pow(cos(DOCKINGPROBE_DROGUE_ANGLE*PI/180),2));
Del = prbP - drgV;
c0 = dotp(mul(M,Del),Del);
c1 = dotp(mul(M,prbD),Del);
c2 = dotp(mul(M,prbD),prbD);
if (c2 != 0){
double delta = c1*c1 - c0*c2;
if (delta < 0) { // no real roots
return 0.0;
} else if (delta == 0) { // tangent to cone
return 0.0;
} else { // two distinct real roots!
double t1 = (-c1 + pow(delta,0.5))/c2;
double t2 = (-c1 - pow(delta,0.5))/c2;
if (dotp(drgA, ((prbP + prbD * t1) - drgV)) > 0) { // Check for proper cone solution
t = t1;
} else {
t = t2;
}
}
}
// TODO: make sure this is checking properly
if (t <= 0 && t >= DOCKINGPROBE_PROBE_LENGTH) return 0.0; // if intersection does not occur in actual probe, do nothing
return t;
}
void blochsim(double *b1real, double *b1imag,
double *xgrad, double *ygrad, double *zgrad, double *tsteps,
int ntime, double *e1, double *e2, double df,
double dx, double dy, double dz,
double *mx, double *my, double *mz, int mode)
/* Go through time for one df and one dx,dy,dz. */
{
int tcount;
double gammadx;
double gammady;
double gammadz;
double rotmat[9];
double amat[9], bvec[3]; /* A and B propagation matrix and vector */
double arot[9], brot[3]; /* A and B after rotation step. */
double decmat[9]; /* Decay matrix for each time step. */
double decvec[3]; /* Recovery vector for each time step. */
double rotx,roty,rotz; /* Rotation axis coordinates. */
double imat[9], mvec[3];
double mcurr0[3]; /* Current magnetization before rotation. */
double mcurr1[3]; /* Current magnetization before decay. */
eyemat(amat); /* A is the identity matrix. */
eyemat(imat); /* I is the identity matrix. */
zerovec(bvec);
zerovec(decvec);
zeromat(decmat);
gammadx = dx*GAMMA; /* Convert to Hz/cm */
gammady = dy*GAMMA; /* Convert to Hz/cm */
gammadz = dz*GAMMA; /* Convert to Hz/cm */
mcurr0[0] = *mx; /* Set starting x magnetization */
mcurr0[1] = *my; /* Set starting y magnetization */
mcurr0[2] = *mz; /* Set starting z magnetization */
for (tcount = 0; tcount < ntime; tcount++)
{
/* Rotation */
rotz = -(*xgrad++ * gammadx + *ygrad++ * gammady + *zgrad++ * gammadz +
df*TWOPI ) * *tsteps;
rotx = (- *b1real++ * GAMMA * *tsteps);
roty = (+ *b1imag++ * GAMMA * *tsteps++);
calcrotmat(rotx, roty, rotz, rotmat);
if (mode == 1)
{
multmats(rotmat,amat,arot);
multmatvec(rotmat,bvec,brot);
}
else
multmatvec(rotmat,mcurr0,mcurr1);
/* Decay */
decvec[2]= 1- *e1;
decmat[0]= *e2;
decmat[4]= *e2++;
decmat[8]= *e1++;
if (mode == 1)
{
multmats(decmat,arot,amat);
multmatvec(decmat,brot,bvec);
addvecs(bvec,decvec,bvec);
}
else
{
multmatvec(decmat,mcurr1,mcurr0);
addvecs(mcurr0,decvec,mcurr0);
}
/*
printf("rotmat = [%6.3f %6.3f %6.3f ] \n",rotmat[0],rotmat[3],
rotmat[6]);
printf(" [%6.3f %6.3f %6.3f ] \n",rotmat[1],rotmat[4],
rotmat[7]);
printf(" [%6.3f %6.3f %6.3f ] \n",rotmat[2],rotmat[5],
rotmat[8]);
printf("A = [%6.3f %6.3f %6.3f ] \n",amat[0],amat[3],amat[6]);
printf(" [%6.3f %6.3f %6.3f ] \n",amat[1],amat[4],amat[7]);
printf(" [%6.3f %6.3f %6.3f ] \n",amat[2],amat[5],amat[8]);
printf(" B = <%6.3f,%6.3f,%6.3f> \n",bvec[0],bvec[1],bvec[2]);
printf("<mx,my,mz> = <%6.3f,%6.3f,%6.3f> \n",
amat[6] + bvec[0], amat[7] + bvec[1], amat[8] + bvec[2]);
printf("\n");
*/
if (mode == 2) /* Sample output at times. */
/* Only do this if transient! */
{
*mx = mcurr0[0];
*my = mcurr0[1];
*mz = mcurr0[2];
mx++;
my++;
mz++;
}
}
/* If only recording the endpoint, either store the last
point, or calculate the steady-state endpoint. */
if (mode==0) /* Indicates start at given m, or m0. */
{
*mx = mcurr0[0];
*my = mcurr0[1];
*mz = mcurr0[2];
}
else if (mode==1) /* Indicates to find steady-state magnetization */
{
scalemat(amat,-1.0); /* Negate A matrix */
addmats(amat,imat,amat); /* Now amat = (I-A) */
invmat(amat,imat); /* Reuse imat as inv(I-A) */
multmatvec(imat,bvec,mvec); /* Now M = inv(I-A)*B */
*mx = mvec[0];
*my = mvec[1];
*mz = mvec[2];
}
}
inline MATRIX3 eyemat() {return eyemat(1.0);} //Identity Matrix
