janwas
5bf9bca9ef
there are too many W4 and "potentially uninitialized", so those are disabled by 0ad_warning_disable.h. the silly "int x = strlen" and very dangerous "int x = (void*)p" (and vice versa) problems are fixed. This was SVN commit r5526.
226 lines
5.6 KiB
C++
226 lines
5.6 KiB
C++
#include "precompiled.h"
|
|
#include "0ad_warning_disable.h"
|
|
# include <math.h>
|
|
|
|
# include "sr.h"
|
|
# include "sr_alg.h"
|
|
|
|
/*-------------------------------------------------------------------*/
|
|
/* Functions to solve polynomials of 2nd, 3rt and 4th degree. */
|
|
/* Source: graphics gems */
|
|
/*-------------------------------------------------------------------*/
|
|
|
|
//change kai:
|
|
//# define aMAXFLOAT 3.40282347E+38F
|
|
# define aMAXFLOAT 3.40282347E+28F
|
|
# define aEPSILON 1e-9
|
|
# define aEPSILON2 0.00001
|
|
# define aISZERO(x) ((x) > -aEPSILON && (x) < aEPSILON)
|
|
# define aCBRT(x) ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
|
|
((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))
|
|
|
|
int sr_solve_quadric_polynomial ( double c[3], double s[2] )
|
|
{
|
|
double p, q, D;
|
|
|
|
// normal form: x^2 + px + q = 0
|
|
p = c[1] / (2*c[2]);
|
|
q = c[0] / c[2];
|
|
|
|
D = p*p - q;
|
|
|
|
if ( aISZERO(D) )
|
|
{ s[0] = -p;
|
|
return 1;
|
|
}
|
|
else if ( D<0 )
|
|
{ return 0;
|
|
}
|
|
else // if (D > 0)
|
|
{ double sqrt_D = sqrt(D);
|
|
s[0] = sqrt_D - p;
|
|
s[1] = - sqrt_D - p;
|
|
return 2;
|
|
}
|
|
}
|
|
|
|
int sr_solve_cubic_polynomial ( double c[4], double s[3] )
|
|
{
|
|
int i, num;
|
|
double sub;
|
|
double A, B, C;
|
|
double sq_A, p, q;
|
|
double cb_p, D;
|
|
|
|
// normal form: x^3 + Ax^2 + Bx + C = 0
|
|
A = c[2] / c[3];
|
|
B = c[1] / c[3];
|
|
C = c[0] / c[3];
|
|
|
|
// substitute x = y - A/3 to eliminate quadric term:
|
|
// x^3 +px + q = 0
|
|
sq_A = A * A;
|
|
p = 1.0/3 * (- 1.0/3 * sq_A + B);
|
|
q = 1.0/2 * (A * (2.0/27 * sq_A - 1.0/3 * B) + C);
|
|
|
|
// use Cardano's formula
|
|
cb_p = p * p * p;
|
|
D = q * q + cb_p;
|
|
|
|
if ( aISZERO(D) )
|
|
{ if ( aISZERO(q) ) // one triple solution
|
|
{ s[0] = 0;
|
|
num = 1;
|
|
}
|
|
else // one single and one double solution
|
|
{ double u = aCBRT(-q);
|
|
s[0] = 2 * u;
|
|
s[1] = - u;
|
|
num = 2;
|
|
}
|
|
}
|
|
else if ( D<0 ) // Casus irreducibilis: three real solutions
|
|
{ double phi = 1.0/3 * acos ( -q/sqrt(-cb_p) );
|
|
double t = 2 * sqrt(-p);
|
|
s[0] = t * cos(phi);
|
|
s[1] = - t * cos(phi + SR_PI / 3);
|
|
s[2] = - t * cos(phi - SR_PI / 3);
|
|
num = 3;
|
|
}
|
|
else /* one real solution */
|
|
{ double sqrt_D = sqrt(D);
|
|
double u = aCBRT(sqrt_D - q);
|
|
double v = - aCBRT(sqrt_D + q);
|
|
s[0] = u + v;
|
|
num = 1;
|
|
}
|
|
|
|
// resubstitute
|
|
sub = 1.0/3 * A;
|
|
for ( i=0; i<num; ++i ) s[i] -= sub;
|
|
return num;
|
|
}
|
|
|
|
int sr_solve_quartic_polynomial ( double c[5], double s[4] )
|
|
{
|
|
double coeffs[4];
|
|
double z, u, v, sub;
|
|
double A, B, C, D;
|
|
double sq_A, p, q, r;
|
|
int i, num;
|
|
|
|
// normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0
|
|
A = c[3] / c[4];
|
|
B = c[2] / c[4];
|
|
C = c[1] / c[4];
|
|
D = c[0] / c[4];
|
|
|
|
// substitute x = y - A/4 to eliminate cubic term:
|
|
// x^4 + px^2 + qx + r = 0
|
|
sq_A = A * A;
|
|
p = - 3.0/8 * sq_A + B;
|
|
q = A * (1.0/8 * sq_A - 1.0/2 * B) + C;
|
|
r = sq_A * (- 3.0/256*sq_A + 1.0/16*B) - 1.0/4*A*C + D;
|
|
|
|
if ( aISZERO(r) ) // no absolute term: y(y^3 + py + q) = 0
|
|
{ coeffs[0] = q;
|
|
coeffs[1] = p;
|
|
coeffs[2] = 0;
|
|
coeffs[3] = 1;
|
|
num = sr_solve_cubic_polynomial ( coeffs, s );
|
|
s[num++] = 0;
|
|
}
|
|
else // solve the resolvent cubic ...
|
|
{ coeffs[0] = 1.0/2 * r * p - 1.0/8 * q * q;
|
|
coeffs[1] = - r;
|
|
coeffs[2] = - 1.0/2 * p;
|
|
coeffs[3] = 1;
|
|
sr_solve_cubic_polynomial ( coeffs, s );
|
|
// ... and take the one real solution ...
|
|
z = s[ 0 ];
|
|
// ... to build two quadric equations
|
|
u = z * z - r;
|
|
v = 2 * z - p;
|
|
|
|
if (aISZERO(u)) u = 0;
|
|
else if (u > 0) u = sqrt(u);
|
|
else return 0;
|
|
|
|
if (aISZERO(v)) v = 0;
|
|
else if (v > 0) v = sqrt(v);
|
|
else return 0;
|
|
|
|
coeffs[0] = z - u;
|
|
coeffs[1] = q < 0 ? -v : v;
|
|
coeffs[2] = 1;
|
|
num = sr_solve_quadric_polynomial ( coeffs, s );
|
|
|
|
coeffs[0]= z + u;
|
|
coeffs[1] = q < 0 ? v : -v;
|
|
coeffs[2] = 1;
|
|
num += sr_solve_quadric_polynomial ( coeffs, s+num );
|
|
}
|
|
|
|
// resubstitute
|
|
sub = 1.0/4 * A;
|
|
for ( i=0; i<num; ++i ) s[i] -= sub;
|
|
return num;
|
|
}
|
|
|
|
float sr_in_ellipse ( float x, float y, float a, float b )
|
|
{
|
|
float cx = x/a;
|
|
float cy = y/b;
|
|
return cx*cx + cy*cy - 1;
|
|
}
|
|
|
|
// replaces (x,y) by its closest point on the ellipse (a,b)
|
|
void sr_get_closest_on_ellipse ( float a, float b, float& x, float& y )
|
|
{
|
|
double c[5], s[4];
|
|
double r = a / b,
|
|
e = 2. * (b - a*r),
|
|
f = 2. * (x * r);
|
|
|
|
if ( fabs(y)<aEPSILON2 )
|
|
{ x = x>0 ? a : -a;
|
|
y = 0;
|
|
return;
|
|
}
|
|
|
|
c [4] = y;
|
|
c [3] = f - e;
|
|
c [2] = 0;
|
|
c [1] = f + e;
|
|
c [0] = -y;
|
|
|
|
int nb = sr_solve_quartic_polynomial ( c, s );
|
|
|
|
double denom, s_theta, c_theta, dist;
|
|
double test_x [4], test_y [4];
|
|
double min_dist = aMAXFLOAT;
|
|
int i, winner=0;
|
|
|
|
// Find the closest point
|
|
for ( i=0; i<nb; i++ )
|
|
{ denom = 1. + s [i] * s [i];
|
|
s_theta = 2. * s [i] / denom;
|
|
c_theta = (1. - s [i] * s [i]) / denom;
|
|
|
|
test_x[i] = a * c_theta;
|
|
test_y[i] = b * s_theta;
|
|
|
|
dist = (test_x[i]-x) * (test_x[i]-x) + (test_y[i]-y) * (test_y[i]-y);
|
|
if ( dist<min_dist )
|
|
{ min_dist = dist;
|
|
winner = i;
|
|
}
|
|
}
|
|
|
|
x = (float)test_x[winner];
|
|
y = (float)test_y[winner];
|
|
}
|
|
|
|
//============================== end of file ===============================
|
|
|