int mode_select(int bsize, int density)
{
return scsi_request("mode_select",0,0,6,12,SCSIR_WRITE,
MODESELECT,0x10,0,0,12,0,
0,0,0,8,
density,B3(0),0,B3(bsize) );
}
static int reallyroutespline (Pedge_t *edges, int edgen,
Ppoint_t *inps, int inpn, Ppoint_t ev0, Ppoint_t ev1) {
Ppoint_t p1, p2, cp1, cp2, p;
Pvector_t v1, v2, splitv, splitv1, splitv2;
double maxd, d, t;
int maxi, i, spliti;
static tna_t *tnas;
static int tnan;
if (tnan < inpn) {
if (!tnas) {
if (!(tnas = (tna_t *)malloc (sizeof (tna_t) * inpn)))
return -1;
} else {
if (!(tnas = (tna_t *)realloc (tnas, sizeof (tna_t) * inpn)))
return -1;
}
tnan = inpn;
}
tnas[0].t = 0;
for (i = 1; i < inpn; i++)
tnas[i].t = tnas[i - 1].t + dist (inps[i], inps[i - 1]);
for (i = 1; i < inpn; i++)
tnas[i].t /= tnas[inpn - 1].t;
for (i = 0; i < inpn; i++) {
tnas[i].a[0] = scale (ev0, B1 (tnas[i].t));
tnas[i].a[1] = scale (ev1, B2 (tnas[i].t));
}
if (mkspline (inps, inpn, tnas, ev0, ev1, &p1, &v1, &p2, &v2) == -1)
return -1;
if (splinefits (edges, edgen, p1, v1, p2, v2, (inpn == 2 ? 1 : 0)))
return 0;
cp1 = add (p1, scale (v1, 1 / 3.0));
cp2 = sub (p2, scale (v2, 1 / 3.0));
for (maxd = -1, maxi = -1, i = 1; i < inpn - 1; i++) {
t = tnas[i].t;
p.x = B0 (t) * p1.x + B1 (t) * cp1.x +
B2 (t) * cp2.x + B3 (t) * p2.x;
p.y = B0 (t) * p1.y + B1 (t) * cp1.y +
B2 (t) * cp2.y + B3 (t) * p2.y;
if ((d = dist (p, inps[i])) > maxd)
maxd = d, maxi = i;
}
spliti = maxi;
splitv1 = normv (sub (inps[spliti], inps[spliti - 1]));
splitv2 = normv (sub (inps[spliti + 1], inps[spliti]));
splitv = normv (add (splitv1, splitv2));
reallyroutespline (edges, edgen, inps, spliti + 1, ev0, splitv);
reallyroutespline (edges, edgen, &inps[spliti], inpn - spliti, splitv, ev1);
return 0;
}
// Shortest cubic spline through 4 on-curve points(chord approximation)
void Bezier::FitSpline(Vec3 p[])
{
// use chord length for shortest(best) cubic spline approximation
float c3 = (p[1] - p[0]).Magnitude();
float c2 = (p[2] - p[1]).Magnitude();
float c1 = (p[3] - p[2]).Magnitude();
// cases where p[1] is close to p[2] might lead to instabilities(need some heuristic)
if (50 * c2 < c1 + c3)
{
p[1] = p[0] + (p[1] - p[0]) * 0.98;
p[2] = p[3] + (p[2] - p[3]) * 0.98;
c3 = c3 * 0.98;
c2 = (p[2] - p[1]).Magnitude();
c1 = c1 * 0.98;
}
float t1 = c1 / (c1 + c2 + c3);
float t2 = (c1 + c2) / (c1 + c2 + c3);
// Solve M * x = y
float m00 = B1(t1);
float m01 = B2(t1);
float m10 = B1(t2);
float m11 = B2(t2);
float detM = m00 * m11 - m01 * m10;
if (fabs(detM) > 1E-3)
{
// y = p - p0 * B0(t) - p3 * B3(t)
Vec3 y1 = p[1] - p[0] * B0(t1) - p[3] * B3(t1);
Vec3 y2 = p[2] - p[0] * B0(t2) - p[3] * B3(t2);
// Minv
float s = 1 / detM;
float n00 = s * m11;
float n01 = -s * m01;
float n10 = -s * m10;
float n11 = s * m00;
// x = Minv * y
Vec3 x1 = y1 * n00 + y2 * n01;
Vec3 x2 = y1 * n10 + y2 * n11;
p[1] = x1;
p[2] = x2;
}
}
int huddraw_help() {
fg_color = B0(COLOR_BLACK);
bg_color = B3(COLOR_WHITE);
int i,l;
for(i=0; HUD_HELP[i].title; i++) {
if (!strcmp(hud_help_page, HUD_HELP[i].title)) {
huddraw_help_text(HUD_HELP[i].text);
return 1;
}
}
char text[4096];
sprintf(text, "No help available for topic \n"
"%s\n"
"\n"
"Use #hud help to see the list of\n"
"available commands and topics \n"
"\n"
"Use #hud help <topic> for help \n"
"on specific topic \n",
hud_help_page);
huddraw_help_text(text);
return 1;
}
static int
rvec_priv_inst( int dummy_rvec, ulong inst )
{
int op, op_ext, b1, b2, b3;
/* unhandled privileged instruction in supervisor mode */
/* IMPORTANT: The GPRs are not available here! */
op = OPCODE_PRIM( inst );
op_ext = OPCODE_EXT( inst );
b1 = B1( inst ); /* bit 6-10 */
b2 = B2( inst ); /* bit 11-15 */
b3 = B3( inst ); /* bit 16-20 */
switch( OPCODE(op,op_ext) ) {
case OPCODE( 31, 370 ): /* tlbia (opt.) */
/* not implemented on the 601,603,604,G3 (G4?) */
break;
case OPCODE( 31, 470 ): /* dcbi rA,rB -- rA=b2 rB=b3 */
printm("dcbi treated as nop\n");
mregs->nip += 4;
return 0;
default:
printm("Unknown privileged instruction, opcode %lX\n", inst);
stop_emulation();
break;
}
mac_exception( 0x700, MOL_BIT(13) );
return 0;
}
void rubikStep(char *step)
{
u8 m=0;
for(m=0;step[m]!=0;m++)
{
switch(step[m])
{
case 7:allright90();break;
case 11:F1();break;
case 12:F2();break;
case 13:F3();break;
case 21:B1();break;
case 22:B2();break;
case 23:B3();break;
case 31:R1();break;
case 32:R2();break;
case 33:R3();break;
case 41:L1();break;
case 42:L2();break;
case 43:L3();break;
case 51:U1();break;
case 52:U2();break;
case 53:U3();break;
case 61:D1();break;
case 62:D2();break;
case 63:D3();break;
default:break;
}
}
}
mat ls_solve_od(const mat &A, const mat &B)
{
int m=A.rows(), n=A.cols(), N=B.cols(), j;
double beta;
mat A2(A), B2(B), B3(n,N), submat, submat2;
vec tmp(n), v;
// it_assert1(m >= n, "The system is under-determined!");
//it_assert1(m == B.rows(), "The number of rows in A must equal the number of rows in B!");
// Perform a Householder QR factorization
for (j=0; j<n; j++) {
house(rvectorize(A2(j, m-1, j, j)), v, beta);
v *= sqrt(beta);
// submat.ref(A2, j,m-1, j,n-1);
submat = A2(j,m-1,j,n-1);
sub_v_vT_m(submat, v);
// submat.ref(B2, j,m-1, 0,N-1);
submat = B2(j,m-1,0,N-1);
sub_v_vT_m(submat, v);
}
// submat.ref(A2, 0,n-1,0,n-1);
// submat2.ref(B2, 0,n-1,0,N-1);
submat = A2(0,n-1,0,n-1);
submat2 = B2(0,n-1,0,N-1);
for (j=0; j<N; j++) {
backward_substitution(submat, submat2.get_col(j), tmp);
B3.set_col(j, tmp);
}
return B3;
}
void f(void) {
int b1 = B1();
if (b1) {
int b2 = B2();
if (b2) {
B3();
} else {
B4();
}
}
B5();
while (B6()) {
B12();
int b14;
do {
B13();
b14 = B14();
} while(b14);
B15();
}
int b7 = B7();
if (b7 || B8()) {
B9();
}
B10();
B11();
}
void huddraw_info_nav(int r) {
char text[256];
fg_color = B0(COLOR_BLACK);
bg_color = B3(COLOR_SAND_YELLOW);
draw_rect(0,r,128,35,1);
fg_color = B2(COLOR_SAND_YELLOW);
draw_rect(35,r+2,12,14,0);
draw_rect(76,r+2,12,14,0);
// coordinates
fg_color = B3(COLOR_REDSTONE_RED);
bg_color = COLOR_TRANSPARENT;
int32_t x = (int32_t)floor(gs.own.x);
int32_t z = (int32_t)floor(gs.own.z);
int32_t x_= (gs.world==&gs.nether) ? x*8 : x/8;
int32_t z_= (gs.world==&gs.nether) ? z*8 : z/8;
char * n_= (gs.world==&gs.nether) ? "Overworld" : "Nether";
draw_text(3, r+ 3, "X");
draw_text(3, r+10, "Z");
draw_text(3, r+19, "Y");
draw_text(3, r+26, "DIR");
sprintf(text, "%9d", x); draw_text(11,r+3,text);
sprintf(text, "%9d", x_); draw_text(53,r+3,text);
sprintf(text, "%9d", z); draw_text(11,r+10,text);
sprintf(text, "%9d", z_); draw_text(53,r+10,text);
sprintf(text, "%9d", (int32_t)floor(gs.own.y)); draw_text(11,r+19,text);
char * dir = "UNKNOWN";
switch(player_direction()) {
case DIR_NORTH : dir = "NORTH"; break;
case DIR_SOUTH : dir = "SOUTH"; break;
case DIR_EAST : dir = "EAST"; break;
case DIR_WEST : dir = "WEST"; break;
}
sprintf(text, "%9s", dir); draw_text(11,r+26,text);
int pos = (42-(strlen(n_)*4-1))/2+49;
draw_text(pos, r+26, n_);
// compass
huddraw_compass(108,r+17,B0(COLOR_BLACK), B3(COLOR_REDSTONE_RED));
}
int huddraw_map() {
int shading[16] = { 3, 3, 3, 3, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2 };
if (!(hud_inv & HUDINVMASK_TUNNEL)) return 0;
bg_color = B0(COLOR_BLACK);
draw_clear();
int32_t x = (int32_t)floor(gs.own.x);
int32_t y = (int32_t)floor(gs.own.y);
int32_t z = (int32_t)floor(gs.own.z);
extent_t ex = { { x-80, y-12, z-80 }, { x+80, y+3, z+80 } };
cuboid_t cb = export_cuboid_extent(ex);
int r,c,i,j;
int32_t off = cb.boff + 16*cb.sa.x + 16;
for(r=0; r<128; r++) {
for(c=0; c<128; c++) {
int poff = off+r*cb.sa.x+c;
for(j=0; j<16; j++) {
if ( cb.data[j][poff].bid ) {
int8_t color = BLOCK_COLORMAP[cb.data[j][poff].bid][cb.data[j][poff].meta];
hud_image[r*128+c] = color*4 + shading[j];
}
}
}
}
for(i=0; i<256; i++) lh_free(cb.data[i]);
hud_image[64*128+64] = 126;
char text[256];
bg_color = B3(COLOR_WHITE);
fg_color = B3(COLOR_REDSTONE_RED);
sprintf(text, "%d,%d", x, z);
draw_text(2, 2, text);
sprintf(text, "Y:%d", y);
draw_text(2, 9, text);
bg_color = COLOR_TRANSPARENT;
huddraw_compass(111, 16, B3(COLOR_GOLD_YELLOW), B3(COLOR_WHITE));
return 1;
}
int huddraw_tunnel() {
if (!(hud_inv & HUDINVMASK_TUNNEL)) return 0;
bg_color = B1(COLOR_NETHER_RED);
draw_clear();
fg_color = B3(COLOR_GOLD_YELLOW);
int32_t x = (int32_t)floor(gs.own.x);
int32_t y = (int32_t)floor(gs.own.y);
int32_t z = (int32_t)floor(gs.own.z);
extent_t ex = { { x-80, y-2, z-80 }, { x+80, y+2, z+80 } };
cuboid_t cb = export_cuboid_extent(ex);
int r,c,i,j;
int32_t off = cb.boff + 16*cb.sa.x + 16;
for(r=0; r<128; r++) {
for(c=0; c<128; c++) {
int poff = off+r*cb.sa.x+c;
int s1=0,s2=0;
for(i=-8; i<8; i++) {
for(j=0; j<5; j++) {
s1+= (cb.data[j][poff-1+i*cb.sa.x].bid != 0)
-2*(cb.data[j][poff+0+i*cb.sa.x].bid != 0)
+ (cb.data[j][poff+1+i*cb.sa.x].bid != 0);
s2+= (cb.data[j][poff+i -cb.sa.x].bid != 0)
-2*(cb.data[j][poff+i ].bid != 0)
+ (cb.data[j][poff+i +cb.sa.x].bid != 0);
}
}
int s = MAX(s1,s2);
if (s>10) hud_image[r*128+c] = B3(COLOR_RED);
if (s>30) hud_image[r*128+c] = B3(COLOR_ORANGE);
if (s>60) hud_image[r*128+c] = B3(COLOR_YELLOW);
if (s>80) hud_image[r*128+c] = B3(COLOR_WHITE);
}
}
for(i=0; i<256; i++) lh_free(cb.data[i]);
hud_image[64*128+64] = B3(COLOR_DIAMOND_BLUE);
char text[256];
sprintf(text, "%d,%d", x, z);
draw_text(2, 2, text);
sprintf(text, "Y:%d", y);
draw_text(2, 10, text);
bg_color = COLOR_TRANSPARENT;
huddraw_compass(111, 16, B3(COLOR_GOLD_YELLOW), B3(COLOR_WHITE));
return 1;
}
void huddraw_info_health(int r) {
char text[256];
fg_color = B0(COLOR_BLACK);
bg_color = B3(COLOR_GRASS_GREEN);
draw_rect(0,r,128,12,1);
bg_color = COLOR_TRANSPARENT;
fg_color = B3(COLOR_REDSTONE_RED);
draw_blit(fonts, FONTS_ICON_HEART, 8, 8, 2, r+2);
fg_color = B0(COLOR_BLACK);
sprintf(text, "%.1f", gs.own.health);
draw_text(12, r+3, text);
fg_color = B2(COLOR_ORANGE);
draw_blit(fonts, FONTS_ICON_FOOD, 8, 8, 33, r+2);
fg_color = B0(COLOR_BLACK);
sprintf(text, "%d (%.1f)", gs.own.food, gs.own.saturation);
draw_text(43, r+3, text);
}
void
CAST5decrypt(const PGPUInt8 *in, PGPUInt8 *out, const PGPUInt32 *xkey)
{
PGPUInt32 l, r, t;
r = (PGPUInt32) in[0]<<24 | (PGPUInt32) in[1]<<16 |
(PGPUInt32) in[2]<<8 | in[3];
l = (PGPUInt32) in[4]<<24 | (PGPUInt32) in[5]<<16 |
(PGPUInt32) in[6]<<8 | in[7];
t = F1(l, xkey, 15); r ^= G1(t);
t = F3(r, xkey, 14); l ^= G3(t);
t = F2(l, xkey, 13); r ^= G2(t);
t = F1(r, xkey, 12); l ^= G1(t);
// Start here if only doing 12 rounds
t = F3(l, xkey, 11); r ^= G3(t);
t = F2(r, xkey, 10); l ^= G2(t);
t = F1(l, xkey, 9); r ^= G1(t);
t = F3(r, xkey, 8); l ^= G3(t);
t = F2(l, xkey, 7); r ^= G2(t);
t = F1(r, xkey, 6); l ^= G1(t);
t = F3(l, xkey, 5); r ^= G3(t);
t = F2(r, xkey, 4); l ^= G2(t);
t = F1(l, xkey, 3); r ^= G1(t);
t = F3(r, xkey, 2); l ^= G3(t);
t = F2(l, xkey, 1); r ^= G2(t);
t = F1(r, xkey, 0); l ^= G1(t);
out[0] = (PGPUInt8) B0(l);
out[1] = (PGPUInt8) B1(l);
out[2] = (PGPUInt8) B2(l);
out[3] = (PGPUInt8) B3(l);
out[4] = (PGPUInt8) B0(r);
out[5] = (PGPUInt8) B1(r);
out[6] = (PGPUInt8) B2(r);
out[7] = (PGPUInt8) B3(r);
}
/*
* Encrypt the 8 bytes at *in into the 8 bytes at *out using the expanded
* key schedule from *xkey.
*/
static void
CAST5encrypt(PGPByte const *in, PGPByte *out, PGPUInt32 const *xkey)
{
PGPUInt32 l, r, t;
l = (PGPUInt32)
in[0]<<24 | (PGPUInt32)in[1]<<16 | (PGPUInt32)in[2]<<8 | in[3];
r = (PGPUInt32)
in[4]<<24 | (PGPUInt32)in[5]<<16 | (PGPUInt32)in[6]<<8 | in[7];
t = F1(r, xkey, 0); l ^= G1(t);
t = F2(l, xkey, 1); r ^= G2(t);
t = F3(r, xkey, 2); l ^= G3(t);
t = F1(l, xkey, 3); r ^= G1(t);
t = F2(r, xkey, 4); l ^= G2(t);
t = F3(l, xkey, 5); r ^= G3(t);
t = F1(r, xkey, 6); l ^= G1(t);
t = F2(l, xkey, 7); r ^= G2(t);
t = F3(r, xkey, 8); l ^= G3(t);
t = F1(l, xkey, 9); r ^= G1(t);
t = F2(r, xkey, 10); l ^= G2(t);
t = F3(l, xkey, 11); r ^= G3(t);
/* Stop here if only doing 12 rounds */
t = F1(r, xkey, 12); l ^= G1(t);
t = F2(l, xkey, 13); r ^= G2(t);
t = F3(r, xkey, 14); l ^= G3(t);
t = F1(l, xkey, 15); r ^= G1(t);
out[0] = B0(r);
out[1] = B1(r);
out[2] = B2(r);
out[3] = B3(r);
out[4] = B0(l);
out[5] = B1(l);
out[6] = B2(l);
out[7] = B3(l);
}
static int _response(Socket_T socket, mysql_packet_t *pkt) {
memset(pkt, 0, sizeof *pkt);
if (socket_read(socket, pkt->buf, 4) < 4) {
socket_setError(socket, "Error receiving server response -- %s", STRERROR);
return FALSE;
}
pkt->len = B3(pkt->buf);
pkt->len = pkt->len > STRLEN ? STRLEN : pkt->len; // Adjust packet length for this buffer
pkt->seq = pkt->buf[3];
pkt->msg = pkt->buf + 4;
if (socket_read(socket, pkt->msg, pkt->len) != pkt->len) {
socket_setError(socket, "Error receiving server response -- %s", STRERROR);
return FALSE;
}
if (*pkt->msg == MYSQL_ERROR) {
unsigned short code = B2(pkt->msg + 1);
unsigned char *err = pkt->msg + 9;
socket_setError(socket, "Server returned error code %d -- %s", code, err);
return FALSE;
}
return TRUE;
}
int huddraw_test() {
int i;
bg_color = B3(COLOR_WHITE);
draw_clear();
fg_color = 119;
for (i=0; i<36; i++) {
int c=(i%6)*10+2;
int r=(i/6)*10+2;
bg_color = i*4+2;
draw_rect(c, r, 8, 8, 1);
}
int x1=95, y1=95, r=29;
double a;
for (a=0; a<2*M_PI; a+=M_PI/6) {
int x2 = x1+(int)(sin(a)*r);
int y2 = y1+(int)(cos(a)*r);
draw_line(x1,y1,x2,y2);
}
return 1;
}
void huddraw_info_inv(int r) {
bg_color = B3(COLOR_PINK);
fg_color = B0(COLOR_BLACK);
draw_rect(0,r,128,22,1);
int present[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int i,j;
for(j=0; hii[j].pos>=0; j++) {
hudinvitem *h = hii+j;
for(i=h->ssid; i<=h->lsid; i++) {
slot_t *s = gs.inv.slots+i;
if (s->item == h->iid) {
float damage = ((float)h->dur-(float)s->damage)/(float)h->dur;
huddraw_info_inv_item(h->pos, damage, r);
present[h->pos] = 1;
}
}
}
for(i=0; i<8; i++)
if (!present[i])
huddraw_info_inv_item(i, -1, r);
}
void huddraw_info_inv_item(int id, float damage, int r) {
int col = 4+31*(id%4);
int row = r+2+10*(id/4);
int fcol = FONTS_ICON_EQ_C+8*(id%4);
int frow = FONTS_ICON_EQ_R+8*(id/4);
bg_color = COLOR_TRANSPARENT;
fg_color = (damage<0) ? B0(COLOR_CLAY_GRAY) : B3(COLOR_DIAMOND_BLUE);
draw_blit(fonts, fcol, frow, 8, 8, col, row);
fg_color = B2(COLOR_CLAY_GRAY);
draw_rect(col+10, row+1, 16, 6, 1);
if (damage >= 0) {
fg_color = B3(COLOR_LAPIS_BLUE);
if (damage < 1.0) fg_color = B3(COLOR_EMERALD_GREEN);
if (damage < 0.5) fg_color = B3(COLOR_GOLD_YELLOW);
if (damage < 0.25) fg_color = B3(COLOR_ORANGE);
if (damage < 0.1) fg_color = B3(COLOR_REDSTONE_RED);
draw_rect(col+10, row+1, 16*damage, 6, 0);
}
}
TEST(KalmanFilterTest, functionality)
{
KalmanFilter kf;
MatrixXd A(1,1); A << 1;
kf.setStateTransitionModel(A);
MatrixXd H(1,1); H << 1;
kf.setObservationModel(H);
MatrixXd Q(1,1); Q << 0.1;
kf.setProcessNoiseCovariance(Q);
MatrixXd R(1,1); R << 10;
kf.setMeasurementNoiseCovariance(R);
kf.validate();
// check if the calculation of an estimate is correct
InputValue measurement(1);
Input in(measurement);
Output out = kf.estimate(in);
EXPECT_EQ(out.size(), 1);
EXPECT_NEAR(out[0].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.0990, 0.0001);
in[0].setValue(5);
EXPECT_NO_THROW(out = kf.estimate(in));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1951, 0.0001);
// missing measurement
InputValue missingValue;
Input inMissing(missingValue);
EXPECT_NO_THROW(out = kf.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.2866, 0.0001);
// another example ---------------------------------
KalmanFilter kf2;
MatrixXd A2(2,2); A2 << 1,0.1,0,1;
kf2.setStateTransitionModel(A2);
MatrixXd H2(1,2); H2 << 0,1;
kf2.setObservationModel(H2);
MatrixXd Q2(2,2); Q2 << 0.1,0,0,0.1;
kf2.setProcessNoiseCovariance(Q2);
MatrixXd R2(1,1); R2 << 10;
kf2.setMeasurementNoiseCovariance(R2);
kf2.validate();
in[0].setValue(1);
EXPECT_NO_THROW(out = kf2.estimate(in));
EXPECT_EQ(out.size(), 2);
EXPECT_NEAR(out[0].getValue(), 0.0, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.0990, 0.0001);
// add control input
MatrixXd B2(2,1); B2 << 0,1;
kf2.setControlInputModel(B2); // needs to be validated
InputValue ctrl(0.5);
Input in_ctrl(ctrl);
kf2.setControlInput(in_ctrl);
in[0].setValue(5);
EXPECT_NO_THROW(kf2.validate());
EXPECT_NO_THROW(out = kf2.estimate(in));
EXPECT_NEAR(out[0].getValue(), 0.0053, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.20098, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.1951, 0.0001);
// pass control input with invalid size
in_ctrl.add(ctrl); // -> u.size = 2
// setting invalid control input throws exception
EXPECT_THROW(kf2.setControlInput(in_ctrl), length_error);
// changing control input model works
MatrixXd B3(2,2); B3 << 0,0,0,0;
EXPECT_NO_THROW(kf2.setControlInputModel(B3));
EXPECT_ANY_THROW(out = kf2.estimate(in)); // not validated
EXPECT_NO_THROW(kf2.validate());
// missing measurement
EXPECT_NO_THROW(out = kf2.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.0651, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.3048, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.2867, 0.0001);
}
/*
* GenerateBezier :
* Use least-squares method to find Bezier control points for region.
*
*/
QPointF* GenerateBezier(const QList<QPointF> &points, int first, int last, qreal *uPrime, FitVector tHat1, FitVector tHat2)
{
int i;
int nPts; /* Number of pts in sub-curve */
qreal C[2][2]; /* Matrix C */
qreal X[2]; /* Matrix X */
qreal det_C0_C1, /* Determinants of matrices */
det_C0_X,
det_X_C1;
qreal alpha_l, /* Alpha values, left and right */
alpha_r;
FitVector tmp; /* Utility variable */
QPointF *curve;
curve = new QPointF[4];
nPts = last - first + 1;
/* Precomputed rhs for eqn */
// FitVector A[nPts][2]
QVector< QVector<FitVector> > A(nPts, QVector<FitVector>(2));
/* Compute the A's */
for (i = 0; i < nPts; ++i) {
FitVector v1, v2;
v1 = tHat1;
v2 = tHat2;
v1.scale(B1(uPrime[i]));
v2.scale(B2(uPrime[i]));
A[i][0] = v1;
A[i][1] = v2;
}
/* Create the C and X matrices */
C[0][0] = 0.0;
C[0][1] = 0.0;
C[1][0] = 0.0;
C[1][1] = 0.0;
X[0] = 0.0;
X[1] = 0.0;
for (i = 0; i < nPts; ++i) {
C[0][0] += (A[i][0]).dot(A[i][0]);
C[0][1] += A[i][0].dot(A[i][1]);
/* C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/
C[1][0] = C[0][1];
C[1][1] += A[i][1].dot(A[i][1]);
FitVector vfirstp1(points.at(first + i));
FitVector vfirst(points.at(first));
FitVector vlast(points.at(last));
tmp = VectorSub(vfirstp1,
VectorAdd(
VectorScale(vfirst, B0(uPrime[i])),
VectorAdd(
VectorScale(vfirst, B1(uPrime[i])),
VectorAdd(
VectorScale(vlast, B2(uPrime[i])),
VectorScale(vlast, B3(uPrime[i]))))));
X[0] += A[i][0].dot(tmp);
X[1] += A[i][1].dot(tmp);
}
/* Compute the determinants of C and X */
det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
det_C0_X = C[0][0] * X[1] - C[0][1] * X[0];
det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
/* Finally, derive alpha values */
if (qFuzzyCompare(det_C0_C1, qreal(0.0))) {
det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
if (qFuzzyCompare(det_C0_C1, qreal(0.0))) {
det_C0_C1 = Zero;
}
}
alpha_l = det_X_C1 / det_C0_C1;
alpha_r = det_C0_X / det_C0_C1;
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
/* (if alpha is 0, you get coincident control points that lead to
* divide by zero in any subsequent NewtonRaphsonRootFind() call. */
if (alpha_l < 1.0e-6 || alpha_r < 1.0e-6) {
qreal dist = distance(points.at(last), points.at(first)) / 3.0;
curve[0] = points.at(first);
curve[3] = points.at(last);
tHat1.scale(dist);
tHat2.scale(dist);
curve[1] = tHat1 + curve[0];
curve[2] = tHat2 + curve[3];
return curve;
}
/* First and last control points of the Bezier curve are */
/* positioned exactly at the first and last data points */
/* Control points 1 and 2 are positioned an alpha distance out */
/* on the tangent vectors, left and right, respectively */
curve[0] = points.at(first);
curve[3] = points.at(last);
tHat1.scale(alpha_l);
tHat2.scale(alpha_r);
curve[1] = tHat1 + curve[0];
curve[2] = tHat2 + curve[3];
return (curve);
}
/*
* GenerateBezier :
* Use least-squares method to find Bezier control points for region.
*
*/
static BezierCurve GenerateBezier(Point2 *d, int first, int last, double *uPrime,
Vector2 tHat1, Vector2 tHat2)
{
int i;
Vector2 A[MAXPOINTS][2]; /* Precomputed rhs for eqn */
int nPts; /* Number of pts in sub-curve */
double C[2][2]; /* Matrix C */
double X[2]; /* Matrix X */
double det_C0_C1, /* Determinants of matrices */
det_C0_X,
det_X_C1;
double alpha_l, /* Alpha values, left and right */
alpha_r;
Vector2 tmp; /* Utility variable */
BezierCurve bezCurve; /* RETURN bezier curve ctl pts */
bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
nPts = last - first + 1;
/* Compute the A's */
for (i = 0; i < nPts; i++) {
Vector2 v1, v2;
v1 = tHat1;
v2 = tHat2;
V2Scale(&v1, B1(uPrime[i]));
V2Scale(&v2, B2(uPrime[i]));
A[i][0] = v1;
A[i][1] = v2;
}
/* Create the C and X matrices */
C[0][0] = 0.0;
C[0][1] = 0.0;
C[1][0] = 0.0;
C[1][1] = 0.0;
X[0] = 0.0;
X[1] = 0.0;
for (i = 0; i < nPts; i++) {
C[0][0] += V2Dot(&A[i][0], &A[i][0]);
C[0][1] += V2Dot(&A[i][0], &A[i][1]);
/* C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/
C[1][0] = C[0][1];
C[1][1] += V2Dot(&A[i][1], &A[i][1]);
tmp = V2SubII(d[first + i],
V2AddII(
V2ScaleIII(d[first], B0(uPrime[i])),
V2AddII(
V2ScaleIII(d[first], B1(uPrime[i])),
V2AddII(
V2ScaleIII(d[last], B2(uPrime[i])),
V2ScaleIII(d[last], B3(uPrime[i]))))));
X[0] += V2Dot(&A[i][0], &tmp);
X[1] += V2Dot(&A[i][1], &tmp);
}
/* Compute the determinants of C and X */
det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
det_C0_X = C[0][0] * X[1] - C[1][0] * X[0];
det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
/* Finally, derive alpha values */
alpha_l = (det_C0_C1 < ZERO_TOLERANCE) ? 0.0 : det_X_C1 / det_C0_C1;
alpha_r = (det_C0_C1 < ZERO_TOLERANCE) ? 0.0 : det_C0_X / det_C0_C1;
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
/* (if alpha is 0, you get coincident control points that lead to
* divide by zero in any subsequent NewtonRaphsonRootFind() call. */
double segLength = V2DistanceBetween2Points(&d[last], &d[first]);
double epsilon = 1.0e-6 * segLength;
if (alpha_l < epsilon || alpha_r < epsilon)
{
/* fall back on standard (probably inaccurate) formula, and subdivide further if needed. */
double dist = segLength / 3.0;
bezCurve[0] = d[first];
bezCurve[3] = d[last];
V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
return (bezCurve);
}
/* First and last control points of the Bezier curve are */
/* positioned exactly at the first and last data points */
/* Control points 1 and 2 are positioned an alpha distance out */
/* on the tangent vectors, left and right, respectively */
bezCurve[0] = d[first];
bezCurve[3] = d[last];
V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
return (bezCurve);
}
Vector3d cross2d(const Vector2d &A, const Vector2d &B, double z)
{
Vector3d A3(A.x(),A.y(),z), B3(B.x(),B.y(),z);
return A3.cross(B3);
}
TEST( Commission , EquivalenceClass )
{
srand(time(NULL));
for( int times = 0 ; times < 100000 ; ++times )
{
vector<tuple<int,int,int>> dataSets;
int sets = rand()%10+5;
bool VALID_FLAG = true;
bool TERM_FLAG = false;
bool LOCK_ERR_FLAG = false ,STOCK_ERR_FLAG = false ,BARREL_ERR_FLAG = false;
for( int i = 0 ; i < sets ; ++i )
{
int l = num() , s = num() , b = num();
if( !L1(l) || !S1(s) || !B1(b) )
VALID_FLAG = false;
if( L2(l) ) TERM_FLAG = true;
if( L3(l) || L4(l) ) LOCK_ERR_FLAG = true;
if( S2(s) || S3(s) ) STOCK_ERR_FLAG = true;
if( B2(b) || B3(b) ) BARREL_ERR_FLAG = true;
dataSets.emplace_back(make_tuple(l,s,b));
if( TERM_FLAG ) break;
}
//add terminator
if( !TERM_FLAG )
if( num() % 4 != 3 ) // 75% chance
{
dataSets.emplace_back(make_tuple(-1,0,0));
TERM_FLAG = true;
}
else
VALID_FLAG = false;
// for any error, result will be invalid
if( LOCK_ERR_FLAG || STOCK_ERR_FLAG || BARREL_ERR_FLAG || !TERM_FLAG )
VALID_FLAG = false;
double result = retrieve(dataSets);
if( VALID_FLAG == true )
{
ASSERT_TRUE( 0 <= result ) << "Result is Valid but return error";
}
else
{
// error code must match the flag
switch(cms_error_code) {
case TERM_ERROR:
ASSERT_TRUE(!TERM_FLAG);
break;
case LOCK_ERROR:
ASSERT_TRUE(LOCK_ERR_FLAG);
break;
case STOCK_ERROR:
ASSERT_TRUE(STOCK_ERR_FLAG);
break;
case BARREL_ERROR:
ASSERT_TRUE(BARREL_ERR_FLAG);
break;
case OK:
break;
default:
ASSERT_TRUE(false) << "Fatel Error ,Code = " << cms_error_code << endl;
}
}
}
}
/* returns 1 if GPRs have been modified */
int
_rvec_io_read( int dummy_rvec, ulong mphys_ioaddr, void *usr_data )
{
ulong ea, cont, inst=mregs->inst_opcode;
int op, op_ext, rD, rA, rB, d;
int flag=0, ret=1;
enum { byte=1, half=2, word=4, len_mask=7, indexed=8, update=16,
zero=32, reverse=64, nop=128, fpinst=256 };
/* do_io_read() is considered FPU-unsafe */
shield_fpu( mregs );
/* break instruction into parts */
op = OPCODE_PRIM( inst ); /* bit 0-5 */
op_ext = OPCODE_EXT( inst );
rD = B1( inst ); /* bit 6-10 */
rA = B2( inst ); /* bit 11-15 */
rB = B3( inst ); /* bit 16-20 */
d = BD( inst ); /* bit 16-31 sign extended */
switch( op ) {
case 34: /* lbz rD,d(rA) */
flag = byte | zero; break;
case 35: /* lbzu rD,d(rA) */
flag = byte | zero | update; break;
case 40: /* lhz rD,d(rA) */
flag = half | zero; break;
case 41: /* lhzu rD,d(rA) */
flag = half | zero | update; break;
case 42: /* lha rD,d(rA) */
flag = half; break;
case 43: /* lhau rD,d(rA) */
flag = half | update; break;
case 32: /* lwz rD,d(rA) */
flag = word | zero; break;
case 33: /* lwzu, rD,d(rA) */
flag = word | zero | update; break;
case 50: /* lfd frD,d(rA) */ /* FPU */
flag = word | fpinst | zero; break;
case 51: /* lfdu frD, d(rA) */ /* FPU */
flag = word | fpinst | update | zero; break;
}
if( !flag && op==31 ) {
switch( op_ext ) { /* lxxx rD,rA,rB */
case 87: /* lbzx rD,rA,rB */
flag = byte | indexed | zero; break;
case 119: /* lbzux rD,rA,rB */
flag = byte | indexed | zero | update; break;
case 279: /* lhzx rD,rA,rB */
flag = half | indexed | zero; break;
case 311: /* lhzux rD,rA,rB */
flag = half | indexed | zero | update; break;
case 343: /* lhax rD,rA,rB */
flag = half | indexed; break;
case 375: /* lhaux rD,rA,rB */
flag = half | indexed | update; break;
case 23: /* lwzx rD,rA,rB */
flag = word | indexed | zero; break;
case 55: /* lwzux rD,rA,rB */
flag = word | indexed | zero | update; break;
case 790: /* lhbrx rS,rA,rB */
flag = half | indexed | zero | reverse; break;
case 534: /* lwbrx rS,rA,rB */
flag = word | indexed | zero | reverse; break;
case 599: /* lfdx frD,rA,rB */ /* FPU */
flag = word | indexed | zero | fpinst; break;
case 631: /* lfdux frD,rA,rB */ /* FPU */
flag = word | indexed | zero | update | fpinst; break;
case 86: /* dcbf rA,rB - cache instruction*/
/* treat as nop if data-translation is off */
flag = (mregs->msr & MSR_DR) ? 0 : nop; break;
}
}
if( flag & len_mask) { /* instruction found */
if( flag & indexed ) { /* lxxx rD,rA,rB */
ea = mregs->gpr[rB];
ea += rA ? mregs->gpr[rA] : 0;
} else { /* lxxx rD,d(rA) */
ea = rA ? mregs->gpr[rA] : 0;
ea += d;
}
/* ea is the mac untranslated address, */
/* mphys_ioaddr is the mac-physical address */
cont = 0;
do_io_read( usr_data, mphys_ioaddr, (flag & len_mask), &cont );
if( flag & byte ){
cont &= 0xff;
} else if( flag & half ) {
cont &= 0xffff;
if( !(flag & zero) ) /* algebraic */
cont |= (cont & 0x8000)? 0xffff0000 : 0;
if( flag & reverse )
cont = bswap_16( cont );
} else if( flag & reverse)
cont = ld_le32(&cont);
if( !(flag & fpinst) )
mregs->gpr[rD] = cont;
else {
/* FPU instruction */
save_fpu_completely( mregs );
ret = 0;
mregs->fpr[rD].h = cont;
/* check for 4K boundary crossings... */
if( ((mphys_ioaddr+4) & 0xfff) < 4 )
printm("emulate_load_data_inst: MMU translation might be bad\n");
do_io_read( usr_data, mphys_ioaddr+4, 4, &cont );
mregs->fpr[rD].l = cont;
reload_tophalf_fpu( mregs );
}
if( (flag & update) && rA && (rA!=rD || (flag & fpinst)) ) {
ret = 1;
mregs->gpr[rA] = ea;
}
}
if( flag )
mregs->nip += 4;
else {
printm("Unimplemented load instruction %08lX\n", inst );
stop_emulation();
}
return ret;
}
/* returns 1 if GPRs have been modified */
int
_rvec_io_write( int dummy_rvec, ulong mphys_ioaddr, void *usr_data )
{
int op, op_ext, rS, rA, rB, d, flag=0, ret=0;
ulong ea, cont, len, inst=mregs->inst_opcode;
enum { byte=1, half=2, word=4, len_mask=7, indexed=8, update=16,
reverse=32, nop=64, fpinst=128 };
/* do_io_write() is considered FPU-unsafe */
shield_fpu( mregs );
/* break instruction into parts */
op = OPCODE_PRIM( inst );
op_ext = OPCODE_EXT( inst );
rS = B1( inst ); /* bit 6-10 */
rA = B2( inst ); /* bit 11-15 */
rB = B3( inst ); /* bit 16-20 */
d = BD( inst ); /* bit 16-31 sign extended */
switch( op ) {
case 38: /* stb rS,d(rA) */
flag = byte ; break;
case 39: /* stbu rS,d(rA) */
flag = byte | update; break;
case 44: /* sth rS,d(rA) */
flag = half ; break;
case 45: /* sthu rS,d(rA) */
flag = half | update; break;
case 36: /* stw rS,d(rA) */
flag = word ; break;
case 37: /* stwud rS,d(rA) */
flag = word | update; break;
case 54: /* stfd frS,d(rA) */ /* FPU */
flag = word | fpinst; break;
case 55: /* stfdu frS,d(rA) */ /* FPU */
flag = word | fpinst | update; break;
}
if( !flag && op==31 ) {
switch( op_ext ) {
case 215: /* stbx rS,rA,rB */
flag = byte | indexed ; break;
case 247: /* stbux rS,rA,rB */
flag = byte | indexed | update; break;
case 407: /* sthx rS,rA,rB */
flag = half | indexed ; break;
case 439: /* sthux rS,rA,rB */
flag = half | indexed | update; break;
case 151: /* stwx rS,rA,rB */
flag = word | indexed ; break;
case 183: /* stwux rS,rA,rB */
flag = word | indexed | update; break;
case 918: /* sthbrx rS,rA,rB */
flag = half | indexed | reverse; break;
case 662: /* stwbrx rS,rA,rB */
flag = word | indexed | reverse; break;
case 727: /* stfdx frS,rA,rB */
/* printm("FPU store inst\n"); */
flag = word | indexed | fpinst; break;
case 759: /* stfdux frS,rA,rB */
/* printm("FPU store inst\n"); */
flag = word | indexed | update | fpinst; break;
}
}
if( flag & len_mask ) { /* instruction found */
if( flag & indexed ) { /* stxxx rS,rA,rB */
ea = mregs->gpr[rB];
ea += rA ? mregs->gpr[rA] : 0;
} else { /* stxxx rS,d(rA) */
ea = rA ? mregs->gpr[rA] : 0;
ea += d;
}
if( !(flag & fpinst ) )
cont = mregs->gpr[rS];
else {
save_fpu_completely( mregs );
cont = mregs->fpr[rS].h;
}
len = flag & len_mask;
if( flag & byte ) {
cont &= 0xff;
} else if( flag & half ) {
if( !(flag & reverse) )
cont &= 0xffff;
else
cont = bswap_16( cont );
} else if( flag & reverse )
cont = ld_le32(&cont);
/* ea is the mac untranslated address,
* mregs->mmu_ioaddr holds the translated address
*/
do_io_write( usr_data, mphys_ioaddr, cont, len );
if( flag & fpinst ) {
if( ((mphys_ioaddr+4) & 0xfff) < 4 )
printm("emulate store data inst: Possible MMU translation error\n");
do_io_write( usr_data, mphys_ioaddr+4, mregs->fpr[rS].l, 4 );
}
if( (flag & update) && rA ) {
mregs->gpr[rA] = ea;
ret = 1;
}
}
if( flag )
mregs->nip += 4;
else {
printm("Unimplemented store instruction %08lX\n", inst );
stop_emulation();
}
return ret;
}
int main()
{
std::cout << "============== Test 1 ==============" << std::endl << std::endl;
DoubleInterval A00(2,3);
DoubleInterval A01(0,1);
DoubleInterval A10(1,2);
DoubleInterval A11(2,3);
DoubleInterval B0(0,120);
DoubleInterval B1(60,240);
DoubleMatrix *A = new DoubleMatrix(2,2);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
DoubleVector *b = new DoubleVector(2, (DoubleInterval)0);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 2 ==============" << std::endl << std::endl;
A00.assign(-1,3);
A = new DoubleMatrix(2,2);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 3 ==============" << std::endl << std::endl;
A00.assign(2,3);
A01.assign(-5,6);
A = new DoubleMatrix(2,2);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(1,3);
B1.assign(3,4);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 4 ==============" << std::endl << std::endl;
A00.assign(-2,1);
A01.assign(1,5);
A = new DoubleMatrix(2,2);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(1,3);
B1.assign(3,4);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 5 ==============" << std::endl << std::endl;
A00.assign(2,3);
A01.assign(3,4);
DoubleInterval A02(1,2);
DoubleInterval A12(0,1);
DoubleInterval A21(6,8);
DoubleInterval A22(4,5);
B0.assign(0,120);
B1.assign(310,440);
DoubleInterval B2(50,120);
A = new DoubleMatrix(3,3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = A02;
(*A)(1,0) = A00;
(*A)(1,1) = A00;
(*A)(1,2) = A12;
(*A)(2,0) = A00;
(*A)(2,1) = A21;
(*A)(2,2) = A22;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
/*std::cout << "============== Test 6 ==============" << std::endl << std::endl;
A00.assign(2,3);
A = new DoubleMatrix(1,1);
(*A)(0,0) = A00;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
B0.assign(4,5);
b = new DoubleVector(1, (DoubleInterval)0);
(*b)[0] = B0;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b, argc, argv);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 7 ==============" << std::endl << std::endl;
A00.assign(1,2);
A = new DoubleMatrix(1,1);
(*A)(0,0) = A00;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
B0.assign(-5,10);
b = new DoubleVector(1, (DoubleInterval)0);
(*b)[0] = B0;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b, argc, argv);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}*/
std::cout << "============== Test 15 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
(*A)(0,0) = A10;
(*A)(0,1) = A11;
(*A)(1,0) = A00;
(*A)(1,1) = A01;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
(*b)[0] = B1;
(*b)[1] = B0;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 16 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
A00.assign(3,4);
A01.assign(1,2);
A10.assign(0,1);
A11.assign(7,8);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(2,4);
B1.assign(-1,1);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 17 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
A00.assign(2,4);
A01.assign(8,10);
A10.assign(2,4);
A11.assign(4,6);
(*A)(0,0) = -A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(4,6);
B1.assign(8,10);
(*b)[0] = -B0;
(*b)[1] = -B1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 18 ==============" << std::endl << std::endl;
A = new DoubleMatrix(4,4);
A00.assign(4,6);
A01.assign(-6,-4);
A10.assign(9,11);
A11.assign(-11,-9);
DoubleInterval A33(-1,1);
(*A)(0,0) = A00;
(*A)(0,1) = A33;
(*A)(0,2) = A33;
(*A)(0,3) = A33;
(*A)(1,0) = A33;
(*A)(1,1) = A01;
(*A)(1,2) = A33;
(*A)(1,3) = A33;
(*A)(2,0) = A33;
(*A)(2,1) = A33;
(*A)(2,2) = A10;
(*A)(2,3) = A33;
(*A)(3,0) = A33;
(*A)(3,1) = A33;
(*A)(3,2) = A33;
(*A)(3,3) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(4, (DoubleInterval)0);
B0.assign(-2,4);
B1.assign(1,8);
B2.assign(-4,10);
DoubleInterval B3(2,12);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
(*b)[3] = B3;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 19 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(-14,14);
B1.assign(-9,9);
B2.assign(-3,3);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 20 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(-14,0);
B1.assign(-9,0);
B2.assign(-3,0);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 21 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(0,14);
B1.assign(0,9);
B2.assign(0,3);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 22 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(2,14);
B1.assign(-9,-3);
B2.assign(-3,1);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 23 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(2,3);
A01.assign(4,5);
A02.assign(1,2);
A10.assign(-6,-5);
A11.assign(-3,-2);
A12.assign(3,4);
DoubleInterval A20(-4,0);
A21.assign(-5,-4);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = A02;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
(*A)(1,2) = A12;
(*A)(2,0) = A20;
(*A)(2,1) = A21;
(*A)(2,2) = A00;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(2,14);
B1.assign(9,300);
B2.assign(3,100);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 24 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
A00.assign(2,3);
A01.assign(-1,1);
A10.assign(0,5);
A11.assign(3,4);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(2,14);
B1.assign(3,9);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
std::cout << "============== Test 25 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
A00.assign(1,1000);
A01.assign(1,1000);
A10.assign(-1000,1);
A11.assign(1,1000);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(1,2);
B1.assign(3,4);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
try
{
get_exact_system(*A,*b);
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
return 0;
}
int
convert_S_records(
FILE *ifp,
char *inm,
FILE *ofp,
char *onm)
{
char buff[512];
char *p;
u8 cksum;
int incksum;
int c;
int len; /* data length of current line */
int rectype; /* record type */
u32 addr;
bool endrecord = FALSE;
buffer_rec tb;
while ( ! endrecord && (fgets(buff, sizeof(buff), ifp)))
{
p = &buff[0];
if (p[strlen(p)-1] == '\n') /* get rid of newline */
p[strlen(p)-1] = '\0';
if (p[strlen(p)-1] == '\r') /* get rid of any CR */
p[strlen(p)-1] = '\0';
tb.dl_count = 0;
if (*p != 'S')
badformat(p, inm, BADFMT);
p++;
if ((rectype = getnibble(&p)) == -1) /* record type */
badformat(buff, inm, BADTYPE);
if ((len = getbyte(&p)) == -1) /* record len */
badformat(buff, inm, BADLEN);
cksum = len;
switch (rectype)
{
case 0x00: /* comment field, ignored */
goto write_it;
case 0x01: /* data record, 16 bit addr */
if ((addr = get2bytes(&p)) == -1L)
badformat(buff, inm, BADADDR);
len -= 3;
goto doit;
case 0x02: /* ... 24 bit addr */
if ((addr = get3bytes(&p)) == -1L)
badformat(buff, inm, BADADDR);
len -= 4;
goto doit;
case 0x03: /* ... 32 bit addr */
if ((addr = get4bytes(&p)) == -1L)
badformat(buff, inm, BADADDR);
len -= 5;
doit:
cksum += B0(addr) + B1(addr) + B2(addr) + B3(addr);
tb.dl_destaddr = addr;
while (len--)
{
if ((c = getbyte(&p)) == -1)
badformat(buff, inm, BADDATA);
cksum += c;
filesum += c;
tb.dl_buf[tb.dl_count++] = c;
}
break;
case 0x07: /* 32 bit end record */
if ((addr = get4bytes(&p)) == -1L)
badformat(buff, inm, BADADDR);
goto end_rec;
case 0x08: /* 24 bit end record */
if ((addr = get3bytes(&p)) == -1L)
badformat(buff, inm, BADADDR);
goto end_rec;
case 0x09: /* 16 bit end record */
if ((addr = get2bytes(&p)) == -1L)
badformat(buff, inm, BADADDR);
end_rec:
cksum += B0(addr) + B1(addr) + B2(addr) + B3(addr);
tb.dl_jumpaddr = addr;
break;
default:
error(0, "unknown Motorola-S record type: 0x%02x", rectype);
badformat(buff, inm, BADTYPE);
break;
}
/*
* Verify checksums are correct in file.
*/
cksum = (~cksum) & 0xff;
if ((incksum = getbyte(&p)) == -1)
badformat(buff, inm, BADCSUM);
if (((u8) incksum) != cksum)
badformat(buff, inm, MISCSUM);
write_it:
if (tb.dl_count)
write_record(&tb, ofp);
}
return 0;
}
void Bezier::recalc(QPointF p)
{
// http://www.flong.com/texts/code/shapers_bez/
// http://www.lemoda.net/maths/bezier-length/index.html,
// arbitrary but reasonable t-values for interior control points
double t0 = 0.3;
double t1 = 0.7;
if (m_drag_cp0) {
double x = (p.x() - m_endpoint0.x() * B0(t0) - m_cp1.x() * B2(t0) - m_endpoint1.x() * B3(t0)) / B1(t0);
double y = (p.y() - m_endpoint0.y() * B0(t0) - m_cp1.y() * B2(t0) - m_endpoint1.y() * B3(t0)) / B1(t0);
m_cp0 = QPointF(x, y);
}
else {
double x = (p.x() - m_endpoint0.x() * B0(t1) - m_cp0.x() * B1(t1) - m_endpoint1.x() * B3(t1)) / B2(t1);
double y = (p.y() - m_endpoint0.y() * B0(t1) - m_cp0.y() * B1(t1) - m_endpoint1.y() * B3(t1)) / B2(t1);
m_cp1 = QPointF(x, y);
}
/*
DebugDialog::debug(QString("ix:%1 p0x:%2,p0y:%3 p1x:%4,p1y:%5 px:%6,py:%7")
.arg(m_drag_cp0)
.arg(m_endpoint0.x())
.arg(m_endpoint0.y())
.arg(m_endpoint1.x())
.arg(m_endpoint1.y())
.arg(p.x())
.arg(p.y())
);
*/
m_isEmpty = false;
}
int main()
{
std::cout << "============== Test 1 ==============" << std::endl << std::endl;
DoubleInterval A00(2,3);
DoubleInterval A01(0,1);
DoubleInterval A10(1,2);
DoubleInterval A11(2,3);
DoubleInterval B0(0,120);
DoubleInterval B1(60,240);
DoubleMatrix *A = new DoubleMatrix(2,2);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A: " << std::endl;
std::cout << *A << std::endl;
DoubleVector *b = new DoubleVector(2, (DoubleInterval)0);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b: " << std::endl;
std::cout << *b << std::endl << std::endl;
DoubleVector *x = new DoubleVector(2, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
//Solution should be [[-120,90], [-60,240]]^T
delete A;
delete b;
delete x;
/*std::cout << "============== Test 2 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
(*A)(0,0) = 2;
(*A)(0,1) = 1;
(*A)(0,2) = -1;
(*A)(1,0) = -3;
(*A)(1,1) = -1;
(*A)(1,2) = 2;
(*A)(2,0) = -2;
(*A)(2,1) = 1;
(*A)(2,2) = 2;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 8;
(*b)[1] = -11;
(*b)[2] = -3;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//Solution should be [2, 3, -1]^T
std::cout << "============== Test 3 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
(*A)(0,0) = 1;
(*A)(0,1) = -2;
(*A)(1,0) = 2;
(*A)(1,1) = -1;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
(*b)[0] = 3;
(*b)[1] = 9;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(2, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//Solution should be [5, 1]^T
std::cout << "============== Test 4 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
(*A)(0,0) = 2;
(*A)(0,1) = 3;
(*A)(0,2) = 1;
(*A)(1,0) = 1;
(*A)(1,1) = 1;
(*A)(1,2) = 1;
(*A)(2,0) = 3;
(*A)(2,1) = 4;
(*A)(2,2) = 2;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 1;
(*b)[1] = 3;
(*b)[2] = 4;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//There shouldn't be an exact solution as it has free variables
std::cout << "============== Test 5 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
(*A)(0,0) = 1;
(*A)(0,1) = 3;
(*A)(0,2) = 1;
(*A)(1,0) = 1;
(*A)(1,1) = 1;
(*A)(1,2) = -1;
(*A)(2,0) = 3;
(*A)(2,1) = 11;
(*A)(2,2) = 5;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 9;
(*b)[1] = 1;
(*b)[2] = 35;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//There shouldn't be an exact solution as it has free variables
std::cout << "============== Test 6 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,2);
(*A)(0,0) = 1;
(*A)(0,1) = 1;
(*A)(1,0) = 2;
(*A)(1,1) = 3;
(*A)(2,0) = 3;
(*A)(2,1) = -2;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 0;
(*b)[1] = 0;
(*b)[2] = 0;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(2, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//There should be an exact solution as even though the system is overdetermined but
//as the Rank is 2 and the amount of unknowns is 2
std::cout << "============== Test 7 ==============" << std::endl << std::endl;
A = new DoubleMatrix(4,4);
(*A)(0,0) = 1;
(*A)(0,1) = 1;
(*A)(0,2) = 1;
(*A)(0,3) = 1;
(*A)(1,0) = 2;
(*A)(1,1) = 3;
(*A)(1,2) = -1;
(*A)(1,3) = -1;
(*A)(2,0) = 3;
(*A)(2,1) = 2;
(*A)(2,2) = 1;
(*A)(2,3) = 1;
(*A)(3,0) = 3;
(*A)(3,1) = 6;
(*A)(3,2) = -1;
(*A)(3,3) = -1;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(4, (DoubleInterval)0);
(*b)[0] = 0;
(*b)[1] = 2;
(*b)[2] = 5;
(*b)[3] = 4;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(4, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//There should be no solutions as the system is inconsistent
std::cout << "============== Test 8 ==============" << std::endl << std::endl;
A = new DoubleMatrix(4,3);
(*A)(0,0) = -1;
(*A)(0,1) = 2;
(*A)(0,2) = -1;
(*A)(1,0) = -2;
(*A)(1,1) = 2;
(*A)(1,2) = 1;
(*A)(2,0) = 3;
(*A)(2,1) = 2;
(*A)(2,2) = 2;
(*A)(3,0) = -3;
(*A)(3,1) = 8;
(*A)(3,2) = 5;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(4, (DoubleInterval)0);
(*b)[0] = 2;
(*b)[1] = 4;
(*b)[2] = 5;
(*b)[3] = 17;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//There should be an exact solution as even though the system is overdetermined but
//as the Rank is 3 and the amount of unknowns is 3
std::cout << "============== Test 9 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,4);
(*A)(0,0) = 1;
(*A)(0,1) = 3;
(*A)(0,2) = 1;
(*A)(0,3) = 1;
(*A)(1,0) = 2;
(*A)(1,1) = -2;
(*A)(1,2) = 1;
(*A)(1,3) = 2;
(*A)(2,0) = 1;
(*A)(2,1) = -5;
(*A)(2,2) = 0;
(*A)(2,3) = 1;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 3;
(*b)[1] = 8;
(*b)[2] = 5;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(4, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//It is an undetermined system, so no exact solution as it has free variables
std::cout << "============== Test 10 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,4);
(*A)(0,0) = 1;
(*A)(0,1) = 0;
(*A)(0,2) = 0;
(*A)(0,3) = 1;
(*A)(1,0) = 0;
(*A)(1,1) = 1;
(*A)(1,2) = 0;
(*A)(1,3) = 1;
(*A)(2,0) = 0;
(*A)(2,1) = 0;
(*A)(2,2) = 1;
(*A)(2,3) = 1;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 1;
(*b)[1] = 1;
(*b)[2] = 1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(4, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 11 ==============" << std::endl << std::endl;
A = new DoubleMatrix(4,4);
(*A)(0,0) = 1;
(*A)(0,1) = -1;
(*A)(0,2) = 1;
(*A)(0,3) = -1;
(*A)(1,0) = -1;
(*A)(1,1) = 1;
(*A)(1,2) = -1;
(*A)(1,3) = 1;
(*A)(2,0) = 1;
(*A)(2,1) = -1;
(*A)(2,2) = 1;
(*A)(2,3) = -1;
(*A)(3,0) = -1;
(*A)(3,1) = 1;
(*A)(3,2) = -1;
(*A)(3,3) = 1;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(4, (DoubleInterval)0);
(*b)[0] = 0;
(*b)[1] = 0;
(*b)[2] = 0;
(*b)[3] = 0;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(4, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//It is an underdetermined system, so no exact solution as it has free variables
//fails here though because no pivoting is done
std::cout << "============== Test 12 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
(*A)(0,0) = 1;
(*A)(0,1) = 2;
(*A)(0,2) = 3;
(*A)(1,0) = 4;
(*A)(1,1) = 5;
(*A)(1,2) = 6;
(*A)(2,0) = 7;
(*A)(2,1) = 8;
(*A)(2,2) = 9;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 0;
(*b)[1] = 0;
(*b)[2] = 0;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//There shouldn't be an exact solution as it has free variables
std::cout << "============== Test 13 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
(*A)(0,0) = 1;
(*A)(0,1) = -1;
(*A)(0,2) = 2;
(*A)(1,0) = 0;
(*A)(1,1) = 0;
(*A)(1,2) = -1;
(*A)(2,0) = 0;
(*A)(2,1) = 2;
(*A)(2,2) = -1;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 8;
(*b)[1] = -11;
(*b)[2] = -3;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//Should gave the same result as test 14 if pivoting has been implemented correctly
std::cout << "============== Test 14 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
(*A)(0,0) = 1;
(*A)(0,1) = -1;
(*A)(0,2) = 2;
(*A)(1,0) = 0;
(*A)(1,1) = 2;
(*A)(1,2) = -1;
(*A)(2,0) = 0;
(*A)(2,1) = 0;
(*A)(2,2) = -1;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
(*b)[0] = 8;
(*b)[1] = -3;
(*b)[2] = -11;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
//Same system of equations as test 13*/
std::cout << "============== Test 15 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
(*A)(0,0) = A10;
(*A)(0,1) = A11;
(*A)(1,0) = A00;
(*A)(1,1) = A01;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
(*b)[0] = B1;
(*b)[1] = B0;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(2, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 16 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
A00.assign(3,4);
A01.assign(1,2);
A10.assign(0,1);
A11.assign(7,8);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(2,4);
B1.assign(-1,1);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(2, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 17 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
A00.assign(2,4);
A01.assign(8,10);
A10.assign(2,4);
A11.assign(4,6);
(*A)(0,0) = -A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(4,6);
B1.assign(8,10);
(*b)[0] = -B0;
(*b)[1] = -B1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(2, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 18 ==============" << std::endl << std::endl;
A = new DoubleMatrix(4,4);
A00.assign(4,6);
A01.assign(-6,-4);
A10.assign(9,11);
A11.assign(-11,-9);
DoubleInterval A33(-1,1);
(*A)(0,0) = A00;
(*A)(0,1) = A33;
(*A)(0,2) = A33;
(*A)(0,3) = A33;
(*A)(1,0) = A33;
(*A)(1,1) = A01;
(*A)(1,2) = A33;
(*A)(1,3) = A33;
(*A)(2,0) = A33;
(*A)(2,1) = A33;
(*A)(2,2) = A10;
(*A)(2,3) = A33;
(*A)(3,0) = A33;
(*A)(3,1) = A33;
(*A)(3,2) = A33;
(*A)(3,3) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(4, (DoubleInterval)0);
B0.assign(-2,4);
B1.assign(1,8);
DoubleInterval B2(-4,10);
DoubleInterval B3(2,12);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
(*b)[3] = B3;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(4, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 19 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(-14,14);
B1.assign(-9,9);
B2.assign(-3,3);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 20 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(-14,0);
B1.assign(-9,0);
B2.assign(-3,0);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 21 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(0,14);
B1.assign(0,9);
B2.assign(0,3);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 22 ==============" << std::endl << std::endl;
A = new DoubleMatrix(3,3);
A00.assign(3.7, 4.3);
A01.assign(-1.5, -0.5);
A10.assign(3.7, 4.3);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(0,2) = (DoubleInterval)0;
(*A)(1,0) = A01;
(*A)(1,1) = A10;
(*A)(1,2) = A01;
(*A)(2,0) = (DoubleInterval)0;
(*A)(2,1) = A01;
(*A)(2,2) = A10;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(3, (DoubleInterval)0);
B0.assign(2,14);
B1.assign(-9,-3);
B2.assign(-3,1);
(*b)[0] = B0;
(*b)[1] = B1;
(*b)[2] = B2;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(3, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
std::cout << "============== Test 23 ==============" << std::endl << std::endl;
A = new DoubleMatrix(2,2);
A00.assign(2,3);
A01.assign(-1,1);
A10.assign(0,5);
A11.assign(3,4);
(*A)(0,0) = A00;
(*A)(0,1) = A01;
(*A)(1,0) = A10;
(*A)(1,1) = A11;
std::cout << "A = " << std::endl;
std::cout << *A << std::endl;
b = new DoubleVector(2, (DoubleInterval)0);
B0.assign(2,14);
B1.assign(3,9);
(*b)[0] = B0;
(*b)[1] = B1;
std::cout << "b = " << std::endl;
std::cout << *b << std::endl << std::endl;
x = new DoubleVector(2, (DoubleInterval)0);
try
{
x = hansen_gaussian_elimination_v1(*A,*b);
if(x != NULL)
{
std::cout << "x = " << std::endl;
std::cout << *x << std::endl << std::endl;
}
}
catch(const std::exception& e)
{
std::cout << e.what() << std::endl << std::endl;
}
catch(std::string& error)
{
std::cout << error << std::endl << std::endl;
}
delete A;
delete b;
delete x;
return 0;
}
double Bezier::yFromT(double t) const
{
// http://www.lemoda.net/maths/bezier-length/index.html
return m_endpoint0.y() * B0(t) + m_cp0.y() * B1(t) + m_cp1.y() * B2(t) + m_endpoint1.y() * B3(t);
}