int SpherePtFromLine(double p1[3], double p2[3], double center[3], double radius, double roots[2])
{
// given a 3D line defined by point p1 and p2, compute roots such that
// p1+root*(p2-p1) is a point of intersection with a sphere of given center and radius
// whose distance to center (of the sphere) equals radius
double d[3], q[3]; // two points on line, line vector, two roots for t
Sub(p2, p1, d); // set d (line vector)
// vector from point on line, p+t*d, to center is p+td-center, or q+td (q = p-center)
// magnitude-squared of this vector is (q+td)**2 = (q**2)+2tdq+(t**2)(d**2)
// in terms of the quadratic equation, a is d**2, b is 2dq, and c is (q**2)-(radius**2)
Sub(p1, center, q);
double a = MagSq(d);
double b = 2*DotProduct(d, q);
double c = MagSq(q)-radius*radius;
return QuadraticRoots(a, b, c, &roots[0], &roots[1]);
}
/* Laplace transform class.
Finds the laplace transform of a quadratic.
*/
bool Laplace::InitQuadratic(Float num, Float de_a, Float de_b, Float de_c) {
Float r1,r2,r1i,r2i;
//first, lets find the roots of the denomenator
QuadraticRoots(de_a,de_b,de_c,r1,r1i,r2,r2i);
if ((r1i>0) || (r2i>0)) {
printf("cant handle complex\n");
return false;
}
if (r1==r2) {
//TODO: handle squares
printf("cant handle square\n");
return false;
}
Float alpha= -r1;
Float beta = -r2;
//Float alpha_i = -r1i;
//Float beta_i = -r2i;
//calculate impulse response (perform the laplace transform)
int done_flag=0;
Float t=0;
m_Impulse.clear();
while (done_flag!=2) {
t+=m_delta_t;
// Float y = ( 1/(beta - alpha) ) * ( exp(-alpha*t) -exp(-beta*t) ) * (num/de_a); //impulse
// if ((done_flag == 0) && (y>0)) done_flag = 1;
// if ((done_flag == 1) && (y< laplace_epsilon)) done_flag = 2;
Float y =(( 1/(beta * alpha) ) + ( exp(-alpha*t) / (alpha*(alpha-beta)) ) + ( exp(-beta*t) / (beta *(beta- alpha)))) * (num/de_a); //step
if (m_Impulse.size()>0)
if (fabs(m_Impulse[m_Impulse.size()-1] - y)< laplace_epsilon)
done_flag = 2;
// printf("y(%f):%f\n",t,y);
m_Impulse.push_back(y);
}
m_Inputs.clear();
m_Inputs.resize(m_Impulse.size()); //create space for the inputs
return true;
}
