int benchmark(void)
{
double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
double a2 = 1.0, b2 = -4.5, c2 = 17.0, d2 = -30.0;
double a3 = 1.0, b3 = -3.5, c3 = 22.0, d3 = -31.0;
double a4 = 1.0, b4 = -13.7, c4 = 1.0, d4 = -35.0;
int solutions;
double output[48] = {0};
double *output_pos = &(output[0]);
/* solve some cubic functions */
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, output);
/* should get 1 solution: 2.5 */
SolveCubic(a2, b2, c2, d2, &solutions, output);
SolveCubic(a3, b3, c3, d3, &solutions, output);
SolveCubic(a4, b4, c4, d4, &solutions, output);
/* Now solve some random equations */
for(a1=1;a1<3;a1++) {
for(b1=10;b1>8;b1--) {
for(c1=5;c1<6;c1+=0.5) {
for(d1=-1;d1>-3;d1--) {
SolveCubic(a1, b1, c1, d1, &solutions, output_pos);
}
}
}
}
return 0;
}
int main(void) {
double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
double a2 = 1.0, b2 = -4.5, c2 = 17.0, d2 = -30.0;
double x[3];
int solutions;
SolveCubic(a1, b1, c1, d1, &solutions, x);
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a2, b2, c2, d2, &solutions, x);
/* should get 1 solution: 2.5 */
return 0;
}
bool EigenValues(const mat3& m, vec3& res)
{
double a = double(1);
double b = -(m[0] + m[4] + m[8]);
double c = (m[0]*m[4] + m[0]*m[8] + m[4]*m[8] - m[7]*m[5] - m[1]*m[3] - m[2]*m[6]);
double d = -(m[0]*m[4]*m[8] - m[0]*m[7]*m[5] - m[1]*m[3]*m[8] + m[1]*m[6]*m[5] + m[2]*m[3]*m[7] - m[2]*m[6]*m[4]);
double in[] = {d, c, b, a};
double out[3];
auto result = SolveCubic(in, out);
if (result == RootFindResult::RESULT_NO_SOLUTION)
return false;
std::sort(out, out+3);
res[0] = (float)out[2];
res[1] = (float)out[1];
res[2] = (float)out[0];
return true;
}
int main(void)
{
__asm__ ("set 0x00000003, %o0\n\t"
"set 0x00000001, %o1\n\t"
"set 0x80000b00, %o2\n\t"
"st %o0, [%o2 + 0x08]\n\t"
"st %o1, [%o2 + 0x04]\n\t");
double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
double a2 = 1.0, b2 = -4.5, c2 = 17.0, d2 = -30.0;
double a3 = 1.0, b3 = -3.5, c3 = 22.0, d3 = -31.0;
double a4 = 1.0, b4 = -13.7, c4 = 1.0, d4 = -35.0;
double x[3];
double X,fl;
int solutions;
int i,n_eq=0;
unsigned long l = 0x3fed0169L;
struct int_sqrt q;
long n = 0;
/* solve soem cubic functions */
printf("********* CUBIC FUNCTIONS ***********\n");
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("1st equation\n");
for(i=0;i<solutions;i++)
//printf(" %f",x[i]); ////OJO
//printf("\n"); ////OJO
/* should get 1 solution: 2.5 */
SolveCubic(a2, b2, c2, d2, &solutions, x);
printf("2nd equation\n");
for(i=0;i<solutions;i++)
//printf(" %f",x[i]); ////OJO
//printf("\n"); ////OJO
SolveCubic(a3, b3, c3, d3, &solutions, x);
printf("3rd equation\n");
for(i=0;i<solutions;i++)
// printf(" %f",x[i]); ////OJO
//printf("\n"); ////OJO
SolveCubic(a4, b4, c4, d4, &solutions, x);
printf("4th equation\n");
for(i=0;i<solutions;i++)
// printf(" %f",x[i]); ////OJO
//printf("\n"); ////OJO
/* Now solve some random equations */
printf("Solving many random equations\n"); ////OJO
for(a1=1;a1<10;a1++) {
for(b1=10;b1>0;b1--) {
for(c1=5;c1<15;c1+=0.5) {
for(d1=-1;d1>-11;d1--) {
SolveCubic(a1, b1, c1, d1, &solutions, x);
n_eq = n_eq++;
//printf("Solutions: 5 (random)"); ////OJO
//for(i=0;i<solutions;i++) ////OJO
//printf(" %f",x[i]); ////OJO
//printf("\n"); ////OJO
}
}
}
}
printf("Solved %d random cubic equations\n",n_eq);
printf("******** 1000 INTEGER SQR ROOTS *********\n");
/* perform some integer square roots */
for (i = 0; i < 1001; ++i)
{
usqrt(i, &q);
// remainder differs on some machines
// printf("sqrt(%3d) = %2d, remainder = %2d\n",
//printf("sqrt(%3d) = %2d\n", ////OJO
// i, q.sqrt); ////OJO
}
usqrt(l, &q);
//printf("\nsqrt(%lX) = %X, remainder = %X\n", l, q.sqrt, q.frac);
//printf("\nsqrt(%lX) = %X\n", l, q.sqrt); ////OJO
printf("** ANGLE CONVERSION DEG to RAD from 0 to 360 **\n");
/* convert some rads to degrees */
for (X = 0.0; X <= 360.0; X += 1.0)
fl= deg2rad(X); /////OJO
//printf("%3.0f degrees = %.12f radians\n", X, deg2rad(X)); /////OJO
puts("");
printf("** ANGLE CONVERSION RAD to DEG from 0.0 to 2PI sampling 1e-6 **\n");
for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 180))
fl=rad2deg(X); ////OJO
//printf("%.12f radians = %3.0f degrees\n", X, rad2deg(X));
printf("Program exits successfully\n");
__asm__ ("set 0x00000000, %o1\n\t"
"set 0x80000b00, %o2\n\t"
"st %o1, [%o2 + 0x04]\n\t");
return 0;
}
int SolveQuartic( 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 = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
if (IsZero(r))
{
/* no absolute term: y(y^3 + py + q) = 0 */
coeffs[ 0 ] = q;
coeffs[ 1 ] = p;
coeffs[ 2 ] = 0;
coeffs[ 3 ] = 1;
num = SolveCubic(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;
(void) SolveCubic(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 (IsZero(u))
u = 0;
else if (u > 0)
u = sqrt(u);
else
return 0;
if (IsZero(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 = SolveQuadric(coeffs, s);
coeffs[ 0 ]= z + u;
coeffs[ 1 ] = q < 0 ? v : -v;
coeffs[ 2 ] = 1;
num += SolveQuadric(coeffs, s + num);
}
/* resubstitute */
sub = 1.0/4 * A;
for (i = 0; i < num; ++i)
s[ i ] -= sub;
return num;
}
/* Both ellipses are assumed to have non-zero radii */
int Int2Elip(Point *IntPts,Ellipse *E1,Ellipse *E2)
{
TMat ElpCirMat1,ElpCirMat2,InvMat,TempMat;
Conic Conic1,Conic2,Conic3,TempConic;
double Roots[3],qRoots[2];
static Circle TestCir = {{0.0,0.0},1.0};
Line TestLine[2];
Point TestPoint;
double PolyCoef[4]; /* coefficients of the polynomial */
double D; /* discriminant: B^2 - AC */
double Phi; /* ellipse rotation */
double m,n; /* ellipse translation */
double Scl; /* scaling factor */
int NumRoots,NumLines;
int CircleInts; /* intersections between line and circle */
int IntCount = 0; /* number of intersections found */
int i,j,k;
/* compute the transformations which turn E1 and E2 into circles */
Elp2Cir(E1,&ElpCirMat1);
Elp2Cir(E2,&ElpCirMat2);
/* compute the inverse transformation of ElpCirMat1 */
InvElp2Cir(E1,&InvMat);
/* Compute the characteristic matrices */
GenEllipseCoefs(E1,&Conic1);
GenEllipseCoefs(E2,&Conic2);
/* Find x such that Det(Conic1 + x Conic2) = 0 */
PolyCoef[0] = -Conic1.C*Conic1.D*Conic1.D + 2.0*Conic1.B*Conic1.D*Conic1.E -
Conic1.A*Conic1.E*Conic1.E - Conic1.B*Conic1.B*Conic1.F +
Conic1.A*Conic1.C*Conic1.F;
PolyCoef[1] = -(Conic2.C*Conic1.D*Conic1.D) -
2.0*Conic1.C*Conic1.D*Conic2.D + 2.0*Conic2.B*Conic1.D*Conic1.E +
2.0*Conic1.B*Conic2.D*Conic1.E - Conic2.A*Conic1.E*Conic1.E +
2.0*Conic1.B*Conic1.D*Conic2.E - 2.0*Conic1.A*Conic1.E*Conic2.E -
2.0*Conic1.B*Conic2.B*Conic1.F + Conic2.A*Conic1.C*Conic1.F +
Conic1.A*Conic2.C*Conic1.F - Conic1.B*Conic1.B*Conic2.F +
Conic1.A*Conic1.C*Conic2.F;
PolyCoef[2] = -2.0*Conic2.C*Conic1.D*Conic2.D - Conic1.C*Conic2.D*Conic2.D +
2.0*Conic2.B*Conic2.D*Conic1.E + 2.0*Conic2.B*Conic1.D*Conic2.E +
2.0*Conic1.B*Conic2.D*Conic2.E - 2.0*Conic2.A*Conic1.E*Conic2.E -
Conic1.A*Conic2.E*Conic2.E - Conic2.B*Conic2.B*Conic1.F +
Conic2.A*Conic2.C*Conic1.F - 2.0*Conic1.B*Conic2.B*Conic2.F +
Conic2.A*Conic1.C*Conic2.F + Conic1.A*Conic2.C*Conic2.F;
PolyCoef[3] = -Conic2.C*Conic2.D*Conic2.D + 2.0*Conic2.B*Conic2.D*Conic2.E -
Conic2.A*Conic2.E*Conic2.E - Conic2.B*Conic2.B*Conic2.F +
Conic2.A*Conic2.C*Conic2.F;
NumRoots = SolveCubic(PolyCoef,Roots);
if (NumRoots == 0)
return 0;
/* we try all the roots, even though it's redundant, so that we
avoid some pathological situations */
for (i=0;i<NumRoots;i++)
{
NumLines = 0;
/* Conic3 = Conic1 + mu Conic2 */
Conic3.A = Conic1.A + Roots[i]*Conic2.A;
Conic3.B = Conic1.B + Roots[i]*Conic2.B;
Conic3.C = Conic1.C + Roots[i]*Conic2.C;
Conic3.D = Conic1.D + Roots[i]*Conic2.D;
Conic3.E = Conic1.E + Roots[i]*Conic2.E;
Conic3.F = Conic1.F + Roots[i]*Conic2.F;
D = Conic3.B*Conic3.B - Conic3.A*Conic3.C;
if (IsZero(Conic3.A) && IsZero(Conic3.B) && IsZero(Conic3.C))
{
/* Case 1 - Single line */
NumLines = 1;
/* compute endpoints of the line, avoiding division by zero */
if (fabs(Conic3.D) > fabs(Conic3.E))
{
TestLine[0].P1.Y = 0.0;
TestLine[0].P1.X = -Conic3.F/(Conic3.D + Conic3.D);
TestLine[0].P2.Y = 1.0;
TestLine[0].P2.X = -(Conic3.E + Conic3.E + Conic3.F)/
(Conic3.D + Conic3.D);
}
else
{
TestLine[0].P1.X = 0.0;
TestLine[0].P1.Y = -Conic3.F/(Conic3.E + Conic3.E);
TestLine[0].P2.X = 1.0;
TestLine[0].P2.Y = -(Conic3.D + Conic3.D + Conic3.F)/
(Conic3.E + Conic3.E);
}
}
else
{
/* use the espresion for Phi that takes atan of the
smallest argument */
Phi = (fabs(Conic3.B + Conic3.B) < fabs(Conic3.A-Conic3.C)?
atan((Conic3.B + Conic3.B)/(Conic3.A - Conic3.C)):
M_PI_2 - atan((Conic3.A - Conic3.C)/(Conic3.B + Conic3.B)))/2.0;
if (IsZero(D))
{
/* Case 2 - Parallel lines */
TempConic = Conic3;
TempMat = IdentMat;
RotateMat(&TempMat,-Phi);
TransformConic(&TempConic,&TempMat);
if (IsZero(TempConic.C)) /* vertical */
{
PolyCoef[0] = TempConic.F;
PolyCoef[1] = 2*TempConic.D;
PolyCoef[2] = TempConic.A;
if ((NumLines=SolveQuadic(PolyCoef,qRoots))!=0)
{
TestLine[0].P1.X = qRoots[0];
TestLine[0].P1.Y = -1.0;
TestLine[0].P2.X = qRoots[0];
TestLine[0].P2.Y = 1.0;
if (NumLines==2)
{
TestLine[1].P1.X = qRoots[1];
TestLine[1].P1.Y = -1.0;
TestLine[1].P2.X = qRoots[1];
TestLine[1].P2.Y = 1.0;
}
}
}
else /* horizontal */
{
PolyCoef[0] = TempConic.F;
PolyCoef[1] = 2*TempConic.E;
PolyCoef[2] = TempConic.C;
if ((NumLines=SolveQuadic(PolyCoef,qRoots))!=0)
{
TestLine[0].P1.X = -1.0;
TestLine[0].P1.Y = qRoots[0];
TestLine[0].P2.X = 1.0;
TestLine[0].P2.Y = qRoots[0];
if (NumLines==2)
{
TestLine[1].P1.X = -1.0;
TestLine[1].P1.Y = qRoots[1];
TestLine[1].P2.X = 1.0;
TestLine[1].P2.Y = qRoots[1];
}
}
}
TempMat = IdentMat;
RotateMat(&TempMat,Phi);
TransformPoint(&TestLine[0].P1,&TempMat);
TransformPoint(&TestLine[0].P2,&TempMat);
if (NumLines==2)
{
TransformPoint(&TestLine[1].P1,&TempMat);
TransformPoint(&TestLine[1].P2,&TempMat);
}
}
else
{
/* Case 3 - Crossing lines */
NumLines = 2;
/* translate the system so that the intersection of the lines
is at the origin */
TempConic = Conic3;
m = (Conic3.C*Conic3.D - Conic3.B*Conic3.E)/D;
n = (Conic3.A*Conic3.E - Conic3.B*Conic3.D)/D;
TempMat = IdentMat;
TranslateMat(&TempMat,-m,-n);
RotateMat(&TempMat,-Phi);
TransformConic(&TempConic,&TempMat);
/* Compute the line endpoints */
TestLine[0].P1.X = sqrt(fabs(1.0/TempConic.A));
TestLine[0].P1.Y = sqrt(fabs(1.0/TempConic.C));
Scl = max(TestLine[0].P1.X,TestLine[0].P1.Y); /* adjust range */
TestLine[0].P1.X /= Scl;
TestLine[0].P1.Y /= Scl;
TestLine[0].P2.X = - TestLine[0].P1.X;
TestLine[0].P2.Y = - TestLine[0].P1.Y;
TestLine[1].P1.X = TestLine[0].P1.X;
TestLine[1].P1.Y = - TestLine[0].P1.Y;
TestLine[1].P2.X = - TestLine[1].P1.X;
TestLine[1].P2.Y = - TestLine[1].P1.Y;
/* translate the lines back */
TempMat = IdentMat;
RotateMat(&TempMat,Phi);
TranslateMat(&TempMat,m,n);
TransformPoint(&TestLine[0].P1,&TempMat);
TransformPoint(&TestLine[0].P2,&TempMat);
TransformPoint(&TestLine[1].P1,&TempMat);
TransformPoint(&TestLine[1].P2,&TempMat);
}
}
/* find the ellipse line intersections */
for (j = 0;j < NumLines;j++)
{
/* transform the line endpts into the circle space of the ellipse */
TransformPoint(&TestLine[j].P1,&ElpCirMat1);
TransformPoint(&TestLine[j].P2,&ElpCirMat1);
/* compute the number of intersections of the transformed line
and test circle */
CircleInts = IntCirLine(&IntPts[IntCount],&TestCir,&TestLine[j]);
if (CircleInts>0)
{
/* transform the intersection points back into ellipse space */
for (k = 0;k < CircleInts;k++)
TransformPoint(&IntPts[IntCount+k],&InvMat);
/* update the number of intersections found */
IntCount += CircleInts;
}
}
}
/* validate the points */
j = IntCount;
IntCount = 0;
for (i = 0;i < j;i++)
{
TestPoint = IntPts[i];
TransformPoint(&TestPoint,&ElpCirMat2);
if (TestPoint.X < 2.0 && TestPoint.Y < 2.0 &&
IsZero(1.0 - sqrt(TestPoint.X*TestPoint.X +
TestPoint.Y*TestPoint.Y)))
IntPts[IntCount++]=IntPts[i];
}
return IntCount;
}
int startup(void)
{
double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
double a2 = 1.0, b2 = -4.5, c2 = 17.0, d2 = -30.0;
double a3 = 1.0, b3 = -3.5, c3 = 22.0, d3 = -31.0;
double a4 = 1.0, b4 = -13.7, c4 = 1.0, d4 = -35.0;
double x[3];
double X;
int solutions;
int i;
unsigned long l = 0x3fed0169L;
struct int_sqrt q;
long n = 0;
/* solve soem cubic functions */
printf("********* CUBIC FUNCTIONS ***********\n");
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
/* should get 1 solution: 2.5 */
SolveCubic(a2, b2, c2, d2, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
SolveCubic(a3, b3, c3, d3, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
SolveCubic(a4, b4, c4, d4, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
/* Now solve some random equations */
for(a1=1;a1<10;a1++) {
for(b1=10;b1>0;b1--) {
for(c1=5;c1<15;c1+=0.5) {
for(d1=-1;d1>-11;d1--) {
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
}
}
}
}
printf("********* INTEGER SQR ROOTS ***********\n");
/* perform some integer square roots */
for (i = 0; i < 1001; ++i)
{
usqrt(i, &q);
// remainder differs on some machines
// printf("sqrt(%3d) = %2d, remainder = %2d\n",
printf("sqrt(%3d) = %2d\n",
i, q.sqrt);
}
usqrt(l, &q);
//printf("\nsqrt(%lX) = %X, remainder = %X\n", l, q.sqrt, q.frac);
printf("\nsqrt(%lX) = %X\n", l, q.sqrt);
printf("********* ANGLE CONVERSION ***********\n");
/* convert some rads to degrees */
for (X = 0.0; X <= 360.0; X += 1.0)
printf("%3.0f degrees = %.12f radians\n", X, deg2rad(X));
puts("");
for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 180))
printf("%.12f radians = %3.0f degrees\n", X, rad2deg(X));
return 0;
}
void SolveKEps(short kimp, int tinc, double sh, double bu, double tr,
double Km, double *tke, double *eps)
{
int it, nit = 0;
double pr, a, b, c, d, ets;
const double c1 = 1.44, c2 = 1.92, tol = 0.00000001, mu = 0.09;
const double tkemin = 0.001, epsmin = 0.000001;
double c3, sb, Ea, epsa;
double t1, t4, t5, t6, t7, t8, t9, t11, t20;
c3 = c2 - 1.;
if (!kimp) {
/* Analytical Version: Explicit K */
/* if (sh + bu > 0. && Km < 1.) Km = 1.;
sh *= Km; bu *= Km; */
pr = sh + bu/* + PP(tr)*/;
ets = *tke + tinc*(sh + bu);
a = tinc*c3;
b = ets + tinc*(*eps - c1*pr);
c = -*eps * ets;
d = b*b - 4.*a*c;
if (d < 0.) {
*eps = epsmin;
*tke = tkemin;
}
else {
*eps = (-b + sqrt(d)) / (2.*a);
*tke = ets - tinc * *eps;
if (*eps < epsmin) *eps = epsmin;
if (*tke < tkemin) *tke = tkemin;
}
}
else {
/*
Analytical Version: Implicit K:
The following formulae have been obtained with MapleV:
*/
pr = sh + PP(bu);
tr = PP(tr);
sb = sh + bu;
if (!*tke || !*eps) {
*tke = tkemin;
*eps = epsmin;
}
Ea = *tke; epsa = *eps;
t1 = c1*pr;
t5 = c1*c1;
t6 = pr*pr;
t8 = t1*sb;
t9 = sb*sb;
t11 = mu*mu;
t20 = tinc*tinc;
a = ((sb-t1)*mu*c3+((t5*t6-2.*t8+t9)*t11+(2.*(t9-t8)*t11+
t11*t9*c3)*c3)*t20)*tinc;
b = -c3*epsa+(-2.*c2*mu*sb*epsa+((2.-c2)*tr*mu*sb+pr*mu*(2.*epsa-
tr*c1))*c1)*t20+(-mu*sb*c2+(2.0*pr*c2-pr)*mu*c1)*tinc*Ea;
t1 = epsa*epsa;
t4 = c1*c1;
t5 = tr*tr;
t7 = sb*mu;
t8 = tinc*tinc;
c = (t1-epsa*tr*c1-t4*t5*t7*t8)*tinc+((2.*c2-1.)*epsa+
(tr*c2*t7+pr*mu*tr*c1)*c1*t8-tinc*c1*pr*mu*c2*Ea)*Ea;
d = (epsa*tinc*c1*tr-Ea*c2*epsa)*Ea;
if (a == 0.) {
fprintf(stderr, "a is equal to zero in SolveKEps. Leaving\n");
exit (0);
}
*tke = SolveCubic(b/a, c/a, d/a);
*eps = *tke *(*tke *mu * tinc *(c1*pr - c2*sb) + epsa) /
(-*tke * c3 + c2*Ea - tinc * c1 * tr);
if (*tke <= tkemin) *tke = tkemin;
if (*eps < epsmin) *eps = epsmin;
}
}
int main(void)
{
double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
double a2 = 1.0, b2 = -4.5, c2 = 17.0, d2 = -30.0;
double a3 = 1.0, b3 = -3.5, c3 = 22.0, d3 = -31.0;
double a4 = 1.0, b4 = -13.7, c4 = 1.0, d4 = -35.0;
int solutions;
int i;
long n = 0;
double output[48] = {0};
double *output_pos = &(output[0]);
int output_count = 0;
int check_output[48] = {-9, 0, -0, -9,
0, -0, -9, 0,
-0, -9, 0, -0,
-8, 0, -0, -8,
0, -0, -8, 0,
-0, -8, 0, -0,
-4, 0, -0, -4,
0, -0, -4, 0,
-0, -4, 0, -0,
-3, 0, -0, -3,
0, -1, -3, 0,
-0, -3, 0, -1};
initialise_board();
start_trigger();
for(n = 0; n < SCALE_FACTOR; ++n)
{
/* solve some cubic functions */
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, output);
/* should get 1 solution: 2.5 */
SolveCubic(a2, b2, c2, d2, &solutions, output);
SolveCubic(a3, b3, c3, d3, &solutions, output);
SolveCubic(a4, b4, c4, d4, &solutions, output);
/* Now solve some random equations */
for(a1=1;a1<3;a1++) {
for(b1=10;b1>8;b1--) {
for(c1=5;c1<6;c1+=0.5) {
for(d1=-1;d1>-3;d1--) {
SolveCubic(a1, b1, c1, d1, &solutions, output_pos);
output_pos += solutions;
output_count += solutions;
}
}
}
}
}
stop_trigger();
/* Verify that we have the correct result. */
int to_return = 0;
for (i = 0; i < output_count; i++) {
if ((int) output[i] != check_output[i]) {
to_return = -1;
break;
}
}
return to_return;
}
int main(void)
{
double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
double x[3];
double X;
int solutions;
int i;
unsigned long l = 0x3fed0169L;
struct int_sqrt q;
long n = 0;
/* solve soem cubic functions */
printf("********* CUBIC FUNCTIONS ***********\n");
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
a1 = 1.0; b1 = -4.5; c1 = 17.0; d1 = -30.0;
/* should get 1 solution: 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
a1 = 1.0; b1 = -3.5; c1 = 22.0; d1 = -31.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
a1 = 1.0; b1 = -13.7; c1 = 1.0; d1 = -35.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
a1 = 3.0; b1 = 12.34; c1 = 5.0; d1 = 12.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
a1 = -8.0; b1 = -67.89; c1 = 6.0; d1 = -23.6;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
a1 = 45.0; b1 = 8.67; c1 = 7.5; d1 = 34.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
a1 = -12.0; b1 = -1.7; c1 = 5.3; d1 = 16.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
/* Now solve some random equations */
for(a1=1;a1<10;a1+=1) {
for(b1=10;b1>0;b1-=.25) {
for(c1=5;c1<15;c1+=0.61) {
for(d1=-1;d1>-5;d1-=.451) {
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[i]);
printf("\n");
}
}
}
}
printf("********* INTEGER SQR ROOTS ***********\n");
/* perform some integer square roots */
for (i = 0; i < 100000; i+=2)
{
usqrt(i, &q);
// remainder differs on some machines
// printf("sqrt(%3d) = %2d, remainder = %2d\n",
printf("sqrt(%3d) = %2d\n",
i, q.sqrt);
}
printf("\n");
for (l = 0x3fed0169L; l < 0x3fed4169L; l++)
{
usqrt(l, &q);
//printf("\nsqrt(%lX) = %X, remainder = %X\n", l, q.sqrt, q.frac);
printf("sqrt(%lX) = %X\n", l, q.sqrt);
}
printf("********* ANGLE CONVERSION ***********\n");
/* convert some rads to degrees */
/* for (X = 0.0; X <= 360.0; X += 1.0) */
for (X = 0.0; X <= 360.0; X += .001)
printf("%3.0f degrees = %.12f radians\n", X, deg2rad(X));
puts("");
/* for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 180)) */
for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 5760))
printf("%.12f radians = %3.0f degrees\n", X, rad2deg(X));
return 0;
}
int main_basicmath_cubic(void)
{
double a1 = 1.0, b1 = -10.5, c1 = 32.0, d1 = -30.0;
double a2 = 1.0, b2 = -4.5, c2 = 17.0, d2 = -30.0;
double a3 = 1.0, b3 = -3.5, c3 = 22.0, d3 = -31.0;
double a4 = 1.0, b4 = -13.7, c4 = 1.0, d4 = -35.0;
double x[3];
double X;
int solutions;
int i;
unsigned long l = 0x3fed0169L;
struct int_sqrt q;
long n = 0;
/* solve soem cubic functions */
pthread_mutex_lock(&mutex_print);
fprintf(fileout_basicmath_cubic,"********* CUBIC FUNCTIONS ***********\n");
/* should get 3 solutions: 2, 6 & 2.5 */
pthread_mutex_unlock(&mutex_print);
SolveCubic(a1, b1, c1, d1, &solutions, x);
pthread_mutex_lock(&mutex_print);
for(i=0;i<solutions;i++)
fprintf(fileout_basicmath_cubic," %f",x[i]);
fprintf(fileout_basicmath_cubic,"\n");
pthread_mutex_unlock(&mutex_print);
/* should get 1 solution: 2.5 */
SolveCubic(a2, b2, c2, d2, &solutions, x);
pthread_mutex_lock(&mutex_print);
for(i=0;i<solutions;i++)
fprintf(fileout_basicmath_cubic," %f",x[i]);
fprintf(fileout_basicmath_cubic,"\n");
pthread_mutex_unlock(&mutex_print);
SolveCubic(a3, b3, c3, d3, &solutions, x);
pthread_mutex_lock(&mutex_print);
for(i=0;i<solutions;i++)
fprintf(fileout_basicmath_cubic," %f",x[i]);
fprintf(fileout_basicmath_cubic,"\n");
pthread_mutex_unlock(&mutex_print);
SolveCubic(a4, b4, c4, d4, &solutions, x);
pthread_mutex_lock(&mutex_print);
for(i=0;i<solutions;i++)
fprintf(fileout_basicmath_cubic," %f",x[i]);
fprintf(fileout_basicmath_cubic,"\n");
pthread_mutex_unlock(&mutex_print);
/*
if(SMALL)
{
for(a1=1;a1<5;a1+=1) {
for(b1=10;b1>0;b1-=1.0) {
for(c1=10;c1<15;c1+=1.0) {
for(d1=-1;d1>-4;d1=d1-0.25) {
SolveCubic(a1, b1, c1, d1, &solutions, x);
pthread_mutex_lock(&mutex_print);
for(i=0;i<solutions;i++)
fprintf(fileout_basicmath_cubic," %f",x[i]);
fprintf(fileout_basicmath_cubic,"\n");
pthread_mutex_unlock(&mutex_print);
}
}
}
}
}
else
{
for(a1=1;a1<5;a1+=0.1) {
for(b1=10;b1>0;b1-=0.1) {
for(c1=10;c1<15;c1+=0.1) {
for(d1=-1;d1>-4;d1=d1-0.25) {
SolveCubic(a1, b1, c1, d1, &solutions, x);
pthread_mutex_lock(&mutex_print);
for(i=0;i<solutions;i++)
fprintf(fileout_basicmath_cubic," %f",x[i]);
fprintf(fileout_basicmath_cubic,"\n");
pthread_mutex_unlock(&mutex_print);
}
}
}
}
}
pthread_mutex_lock(&mutex_print);
fprintf(fileout_basicmath_cubic,"********* INTEGER SQR ROOTS ***********\n");
for (i = 0; i < 501; ++i)
{
usqrt(i, &q);
fprintf(fileout_basicmath_cubic,"sqrt(%3d) = %2d\n",
i, q.sqrt);
}
pthread_mutex_unlock(&mutex_print);
usqrt(l, &q);
pthread_mutex_lock(&mutex_print);
fprintf(fileout_basicmath_cubic,"\nsqrt(%lX) = %X\n", l, q.sqrt);
pthread_mutex_unlock(&mutex_print);
*/
pthread_mutex_lock(&mutex_print);
fprintf(fileout_basicmath_cubic,"********* ANGLE CONVERSION ***********\n");
pthread_mutex_unlock(&mutex_print);
double res;
for (X = 0.0; X <= 360.0; X += 1.0)
{
res = deg2rad(X);
pthread_mutex_lock(&mutex_print);
fprintf(fileout_basicmath_cubic,"%3.0f degrees = %.12f radians\n", X, res);
pthread_mutex_unlock(&mutex_print);
}
for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 180))
{
res = rad2deg(X);
pthread_mutex_lock(&mutex_print);
fprintf(fileout_basicmath_cubic,"%.12f radians = %3.0f degrees\n", X, res);
pthread_mutex_unlock(&mutex_print);
}
return 0;
}
int main(void)
{
double a1 = 1.0f, b1 = -10.5f, c1 = 32.0f, d1 = -30.0f;
double a2 = 1.0f, b2 = -4.5f, c2 = 17.0f, d2 = -30.0f;
double a3 = 1.0f, b3 = -3.5f, c3 = 22.0f, d3 = -31.0f;
double a4 = 1.0f, b4 = -13.7f, c4 = 1.0f, d4 = -35.0f;
double x[3];
double X;
int solutions;
int i;
unsigned long l = 0x3fed0169L;
struct int_sqrt q;
long n = 0;
/* solve soem cubic functions */
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, x);
SolveCubic(a2, b2, c2, d2, &solutions, x);
SolveCubic(a3, b3, c3, d3, &solutions, x);
SolveCubic(a4, b4, c4, d4, &solutions, x);
/* Now solve some random equations */
_Pragma( "loopbound min 5 max 5" )
for(a1=1;a1<10;a1+=2) {
_Pragma( "loopbound min 5 max 5" )
for(b1=10;b1>0;b1-=2) {
_Pragma( "loopbound min 7 max 7" )
for(c1=5;c1<15;c1+=1.5f) {
_Pragma( "loopbound min 5 max 5" )
for(d1=-1;d1>-11;d1-=2) {
SolveCubic(a1, b1, c1, d1, &solutions, x);
}
}
}
}
/* perform some integer square roots */
_Pragma( "loopbound min 1000 max 1000" )
for (i = 1; i < 1001; i+=1) {
usqrt(i, &q);
// remainder differs on some machines
}
usqrt(l, &q);
/* convert some rads to degrees */
_Pragma( "loopbound min 361 max 361" )
for (X = 0.0f; X <= 360.0f; X += 1.0f) {
deg2rad(X);
}
_Pragma( "loopbound min 360 max 360" )
for (X = 0.0f; X <= (2 * PI + 1e-6f); X += (PI / 180)) {
rad2deg(X);
}
return 0;
}
Result SolveQuartic(double c[5], double out[4])
{
double A = c[3] / c[4];
double B = c[2] / c[4];
double C = c[1] / c[4];
double D = c[0] / c[4];
double p = B - 3.0/8.0*A*A;
double q = A*A*A / 8.0 - A*B/2.0 + C;
double r = -3.0/256.0*A*A*A*A + A*A*B/16.0 - A*C/4.0 + D;
double c0[] = {r*p/2.0 - q*q/8.0, -r, -p/2.0, 1.0};
double out0[4];
Result res = SolveCubic(c0, out0);
if (res == RESULT_NO_SOLUTION)
return RESULT_NO_SOLUTION;
double z = out0[0];
double out1[4];
double out2[4];
Result res1, res2;
if (q >= 0)
{
double c1[] = {z - sqrt(z*z-r), sqrt(2.0*z-p), 1};
double c2[] = {z + sqrt(z*z-r),-sqrt(2.0*z-p), 1};
res1 = SolveQuadric(c1, out1);
res2 = SolveQuadric(c2, out2);
}
else
{
double c1[] = {z + sqrt(z*z-r), sqrt(2.0*z-p), 1};
double c2[] = {z - sqrt(z*z-r),-sqrt(2.0*z-p), 1};
res1 = SolveQuadric(c1, out1);
res2 = SolveQuadric(c2, out2);
}
if (res1 == RESULT_NO_SOLUTION && res2 == RESULT_NO_SOLUTION)
return RESULT_NO_SOLUTION;
if (res1 == RESULT_ONE_SOLUTION && res2 == RESULT_NO_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
return RESULT_ONE_SOLUTION;
}
if (res1 == RESULT_NO_SOLUTION && res2 == RESULT_ONE_SOLUTION)
{
out[0] = out2[0] - A / 4.0;
return RESULT_ONE_SOLUTION;
}
if (res1 == RESULT_ONE_SOLUTION && res2 == RESULT_ONE_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
out[1] = out2[0] - A / 4.0;
return RESULT_TWO_SOLUTIONS;
}
if (res1 == RESULT_TWO_SOLUTIONS && res2 == RESULT_NO_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
out[1] = out1[1] - A / 4.0;
return RESULT_TWO_SOLUTIONS;
}
if (res1 == RESULT_NO_SOLUTION && res2 == RESULT_TWO_SOLUTIONS)
{
out[0] = out2[0] - A / 4.0;
out[1] = out2[1] - A / 4.0;
return RESULT_TWO_SOLUTIONS;
}
if (res1 == RESULT_ONE_SOLUTION && res2 == RESULT_TWO_SOLUTIONS)
{
out[0] = out1[0] - A / 4.0;
out[1] = out2[0] - A / 4.0;
out[2] = out2[1] - A / 4.0;
return RESULT_THREE_SOLUTIONS;
}
if (res1 == RESULT_TWO_SOLUTIONS && res2 == RESULT_ONE_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
out[1] = out1[1] - A / 4.0;
out[2] = out2[0] - A / 4.0;
return RESULT_THREE_SOLUTIONS;
}
if (res1 == RESULT_TWO_SOLUTIONS && res2 == RESULT_TWO_SOLUTIONS)
{
out[0] = out1[0] - A / 4.0;
out[1] = out1[1] - A / 4.0;
out[2] = out2[0] - A / 4.0;
out[3] = out2[1] - A / 4.0;
return RESULT_FOUR_SOLUTIONS;
}
std::cout << "Something wrong in "__FUNCSIG__ << std::endl;
return RESULT_NO_SOLUTION;
}
int main(void)
{
double a1 = 1.0;
__boundcheck_metadata_store((void *)(&a1),(void *)((size_t)(&a1)+sizeof(a1)*8-1));
double b1 = -10.5;
__boundcheck_metadata_store((void *)(&b1),(void *)((size_t)(&b1)+sizeof(b1)*8-1));
double c1 = 32.0;
__boundcheck_metadata_store((void *)(&c1),(void *)((size_t)(&c1)+sizeof(c1)*8-1));
double d1 = -30.0;
__boundcheck_metadata_store((void *)(&d1),(void *)((size_t)(&d1)+sizeof(d1)*8-1));
double x[3];__boundcheck_metadata_store(&x[0],&x[3-1]);
double X;
__boundcheck_metadata_store((void *)(&X),(void *)((size_t)(&X)+sizeof(X)*8-1));
int solutions;
__boundcheck_metadata_store((void *)(&solutions),(void *)((size_t)(&solutions)+sizeof(solutions)*8-1));
int i;
__boundcheck_metadata_store((void *)(&i),(void *)((size_t)(&i)+sizeof(i)*8-1));
unsigned long l = 0x3fed0169L;
__boundcheck_metadata_store((void *)(&l),(void *)((size_t)(&l)+sizeof(l)*8-1));
struct int_sqrt q;
__boundcheck_metadata_store((void *)(&q),(void *)((size_t)(&q)+sizeof(q)*8-1));
long n = 0;
__boundcheck_metadata_store((void *)(&n),(void *)((size_t)(&n)+sizeof(n)*8-1));
/* solve soem cubic functions */
printf("********* CUBIC FUNCTIONS ***********\n");
/* should get 3 solutions: 2, 6 & 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,23,18,"main",i)]);
printf("\n");
a1 = 1.0; b1 = -4.5; c1 = 17.0; d1 = -30.0;
/* should get 1 solution: 2.5 */
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,31,18,"main",i)]);
printf("\n");
a1 = 1.0; b1 = -3.5; c1 = 22.0; d1 = -31.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,38,18,"main",i)]);
printf("\n");
a1 = 1.0; b1 = -13.7; c1 = 1.0; d1 = -35.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,45,18,"main",i)]);
printf("\n");
a1 = 3.0; b1 = 12.34; c1 = 5.0; d1 = 12.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,52,18,"main",i)]);
printf("\n");
a1 = -8.0; b1 = -67.89; c1 = 6.0; d1 = -23.6;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,59,18,"main",i)]);
printf("\n");
a1 = 45.0; b1 = 8.67; c1 = 7.5; d1 = 34.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,66,18,"main",i)]);
printf("\n");
a1 = -12.0; b1 = -1.7; c1 = 5.3; d1 = 16.0;
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,73,18,"main",i)]);
printf("\n");
/* Now solve some random equations */
for(a1=1;a1<10;a1+=1) {
for(b1=10;b1>0;b1-=.25) {
for(c1=5;c1<15;c1+=0.61) {
for(d1=-1;d1>-5;d1-=.451) {
SolveCubic(a1, b1, c1, d1, &solutions, x);
printf("Solutions:");
for(i=0;i<solutions;i++)
printf(" %f",x[_RV_insert_check(0,3,84,18,"main",i)]);
printf("\n");
}
}
}
}
printf("********* INTEGER SQR ROOTS ***********\n");
/* perform some integer square roots */
for (i = 0; i < 100000; i+=2)
{
usqrt(i, &q);
// remainder differs on some machines
// printf("sqrt(%3d) = %2d, remainder = %2d\n",
printf("sqrt(%3d) = %2d\n",
i, q.sqrt);
}
printf("\n");
for (l = 0x3fed0169L; l < 0x3fed4169L; l++)
{
usqrt(l, &q);
//printf("\nsqrt(%lX) = %X, remainder = %X\n", l, q.sqrt, q.frac);
printf("sqrt(%lX) = %X\n", l, q.sqrt);
}
printf("********* ANGLE CONVERSION ***********\n");
/* convert some rads to degrees */
/* for (X = 0.0; X <= 360.0; X += 1.0) */
for (X = 0.0; X <= 360.0; X += .001)
printf("%3.0f degrees = %.12f radians\n", X, deg2rad(X));
puts("");
/* for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 180)) */
for (X = 0.0; X <= (2 * PI + 1e-6); X += (PI / 5760))
printf("%.12f radians = %3.0f degrees\n", X, rad2deg(X));
return 0;
}
