PMatrix matrix_copy(PMatrix pm) { PMatrix tpm = matrix_make(pm->row, pm->column); memcpy_s(tpm->matrix, pm->row * pm->column * sizeof(double), pm->matrix, pm->row * pm->column * sizeof(double)); return tpm; }
PMatrix matrix_make_rotate2_real(double real) { PMatrix pm = matrix_make(2, 2); pm->matrix[0][0] = pm->matrix[1][1] = cos(real); pm->matrix[1][0] = sin(real); pm->matrix[0][1] = -pm->matrix[1][0]; return pm; }
PMatrix matrix_make_unit(size_t unit_size) { PMatrix pm = matrix_make(unit_size, unit_size); size_t r, c; for (r = 0; r < unit_size; r++) for (c = 0; c < unit_size; c++) r[pm->matrix][c] = r == c ? 1 : 0; return pm; }
PMatrix matrix_transpose(PMatrix target) { PMatrix pm; size_t r, c; pm = matrix_make(target->column, target->row); for (r = 0; r < target->row; r++) for (c = 0; c < target->column; c++) pm->matrix[c][r] = target->matrix[r][c]; return pm; }
PMatrix matrix_constant_divide(PMatrix pm, double value) { PMatrix pmt; size_t r, c; pmt = matrix_make(pm->row, pm->column); for (r = 0; r < pm->row; r++) for (c = 0; c < pm->column; c++) pmt->matrix[r][c] = pm->matrix[r][c] / value; return pmt; }
PMatrix matrix_each_minus(PMatrix pm_left, PMatrix pm_right) { PMatrix pm; size_t r, c; if (!matrix_check_row_column(pm_left, pm_right)) return MATRIX_ERROR; pm = matrix_make(pm_left->row, pm_right->column); for (r = 0; r < pm_left->row; r++) for (c = 0; c < pm_left->column; c++) pm->matrix[r][c] = pm_left->matrix[r][c] - pm_right->matrix[r][c]; return pm; }
/* Used to create the rotation matrix. Matrix could be taken from for example .DXF files directly (3D Studio animations include matrix for each frame! */ void creatematrix(matrix dest, matrix imatrix, signed long frame) { matrix tmatrix; // create xy-axis rotation matrix matrix_make(tmatrix,imatrix); tmatrix[0]=cosi[frame]; // y-angle tmatrix[2]=-sini[frame]; // ya tmatrix[4]=sini[frame]*sini[frame] >> 8; // ya*xa tmatrix[5]=cosi[frame]; // xa tmatrix[6]=sini[frame]*cosi[frame] >> 8; //sx*cy tmatrix[8]=cosi[frame]*sini[frame] >> 8; // cx*sy tmatrix[9]=-sini[frame]; // -sx tmatrix[10]=cosi[frame]*cosi[frame] >> 8; //cx*cy matrixmul(dest,imatrix,tmatrix); dest[14]=140; }
PMatrix matrix_make_rotate4y_real(double real) { PMatrix pm = matrix_make(4, 4); pm->matrix[0][0] = pm->matrix[2][2] = cos(real); pm->matrix[0][2] = sin(real); pm->matrix[2][0] = -pm->matrix[0][2]; pm->matrix[1][1] = pm->matrix[3][3] = 1.0f; pm->matrix[0][1] = pm->matrix[0][3] = pm->matrix[1][0] = pm->matrix[1][2] = pm->matrix[1][3] = pm->matrix[2][1] = pm->matrix[2][3] = pm->matrix[3][0] = pm->matrix[3][1] = pm->matrix[3][2] = 0.0f; return pm; }
/* * IBM 4704 Scan Code */ uint8_t matrix_scan(void) { uint8_t code = ibm4704_recv(); if (code==0xFF) { // Not receivd return 0; } else if ((code&0x7F) >= 0x7A) { // 0xFF-FA and 0x7F-7A is not scancode xprintf("Error: %02X\n", code); matrix_clear(); return 0; } else if (code&0x80) { dprintf("%02X\n", code); matrix_make(code); } else { dprintf("%02X\n", code); matrix_break(code); } return 1; }
PMatrix matrix_multiple(PMatrix pm_left, PMatrix pm_right) { PMatrix pm; size_t r, c, k; if (!matrix_check_row_column_multiple(pm_left, pm_right)) return MATRIX_ERROR; pm = matrix_make(pm_left->row, pm_right->column); for(r = 0; r < pm_left->row; r++) for(c = 0; c < pm_right->column; c++) { double sum = 0.0f; for (k = 0; k < pm_right->row; k++) sum += pm_left->matrix[r][k] * pm_right->matrix[k][c]; pm->matrix[r][c] = sum; } return pm; }
uint8_t matrix_scan(void) { static enum { INIT, E0, // Pause: E1 1D 45, E1 9D C5 E1, E1_1D, E1_9D, } state = INIT; uint8_t code = xt_host_recv(); if (!code) return 0; dprintf("%02X ", code); switch (state) { case INIT: switch (code) { case 0xE0: state = E0; break; case 0xE1: state = E1; break; default: if (code < 0x80) matrix_make(code); else matrix_break(code & 0x7F); break; } break; case E0: switch (code) { case 0x2A: case 0xAA: case 0x36: case 0xB6: //ignore fake shift state = INIT; break; default: if (code < 0x80) matrix_make(move_e0code(code)); else matrix_break(move_e0code(code & 0x7F)); state = INIT; break; } break; case E1: switch (code) { case 0x1D: state = E1_1D; break; case 0x9D: state = E1_9D; break; default: state = INIT; break; } break; case E1_1D: switch (code) { case 0x45: matrix_make(0x55); break; default: state = INIT; break; } break; case E1_9D: switch (code) { case 0x45: matrix_break(0x55); break; default: state = INIT; break; } break; default: state = INIT; } return 1; }
/* * PS/2 Scan Code Set 2: Exceptional Handling * * There are several keys to be handled exceptionally. * The scan code for these keys are varied or prefix/postfix'd * depending on modifier key state. * * Keyboard Scan Code Specification: * http://www.microsoft.com/whdc/archive/scancode.mspx * http://download.microsoft.com/download/1/6/1/161ba512-40e2-4cc9-843a-923143f3456c/scancode.doc * * * 1) Insert, Delete, Home, End, PageUp, PageDown, Up, Down, Right, Left * a) when Num Lock is off * modifiers | make | break * ----------+---------------------------+---------------------- * Ohter | <make> | <break> * LShift | E0 F0 12 <make> | <break> E0 12 * RShift | E0 F0 59 <make> | <break> E0 59 * L+RShift | E0 F0 12 E0 F0 59 <make> | <break> E0 59 E0 12 * * b) when Num Lock is on * modifiers | make | break * ----------+---------------------------+---------------------- * Other | E0 12 <make> | <break> E0 F0 12 * Shift'd | <make> | <break> * * Handling: These prefix/postfix codes are ignored. * * * 2) Keypad / * modifiers | make | break * ----------+---------------------------+---------------------- * Ohter | <make> | <break> * LShift | E0 F0 12 <make> | <break> E0 12 * RShift | E0 F0 59 <make> | <break> E0 59 * L+RShift | E0 F0 12 E0 F0 59 <make> | <break> E0 59 E0 12 * * Handling: These prefix/postfix codes are ignored. * * * 3) PrintScreen * modifiers | make | break * ----------+--------------+----------------------------------- * Other | E0 12 E0 7C | E0 F0 7C E0 F0 12 * Shift'd | E0 7C | E0 F0 7C * Control'd | E0 7C | E0 F0 7C * Alt'd | 84 | F0 84 * * Handling: These prefix/postfix codes are ignored, and both scan codes * 'E0 7C' and 84 are seen as PrintScreen. * * 4) Pause * modifiers | make(no break code) * ----------+-------------------------------------------------- * Other | E1 14 77 E1 F0 14 F0 77 * Control'd | E0 7E E0 F0 7E * * Handling: Both code sequences are treated as a whole. * And we need a ad hoc 'pseudo break code' hack to get the key off * because it has no break code. * */ uint8_t matrix_scan(void) { // scan code reading states static enum { INIT, F0, E0, E0_F0, // Pause E1, E1_14, E1_14_77, E1_14_77_E1, E1_14_77_E1_F0, E1_14_77_E1_F0_14, E1_14_77_E1_F0_14_F0, // Control'd Pause E0_7E, E0_7E_E0, E0_7E_E0_F0, } state = INIT; is_modified = false; // 'pseudo break code' hack if (matrix_is_on(ROW(PAUSE), COL(PAUSE))) { matrix_break(PAUSE); } uint8_t code = ps2_host_recv(); if (!ps2_error) { switch (state) { case INIT: switch (code) { case 0xE0: state = E0; break; case 0xF0: state = F0; break; case 0xE1: state = E1; break; case 0x83: // F7 matrix_make(F7); state = INIT; break; case 0x84: // Alt'd PrintScreen matrix_make(PRINT_SCREEN); state = INIT; break; case 0x00: // Overrun [3]p.25 matrix_clear(); clear_keyboard(); print("Overrun\n"); state = INIT; break; default: // normal key make if (code < 0x80) { matrix_make(code); } else { matrix_clear(); clear_keyboard(); xprintf("unexpected scan code at INIT: %02X\n", code); } state = INIT; } break; case E0: // E0-Prefixed switch (code) { case 0x12: // to be ignored case 0x59: // to be ignored state = INIT; break; case 0x7E: // Control'd Pause state = E0_7E; break; case 0xF0: state = E0_F0; break; default: if (code < 0x80) { matrix_make(code|0x80); } else { matrix_clear(); clear_keyboard(); xprintf("unexpected scan code at E0: %02X\n", code); } state = INIT; } break; case F0: // Break code switch (code) { case 0x83: // F7 matrix_break(F7); state = INIT; break; case 0x84: // Alt'd PrintScreen matrix_break(PRINT_SCREEN); state = INIT; break; case 0xF0: matrix_clear(); clear_keyboard(); xprintf("unexpected scan code at F0: F0(clear and cont.)\n"); break; default: if (code < 0x80) { matrix_break(code); } else { matrix_clear(); clear_keyboard(); xprintf("unexpected scan code at F0: %02X\n", code); } state = INIT; } break; case E0_F0: // Break code of E0-prefixed switch (code) { case 0x12: // to be ignored case 0x59: // to be ignored state = INIT; break; default: if (code < 0x80) { matrix_break(code|0x80); } else { matrix_clear(); clear_keyboard(); xprintf("unexpected scan code at E0_F0: %02X\n", code); } state = INIT; } break; // following are states of Pause case E1: switch (code) { case 0x14: state = E1_14; break; default: state = INIT; } break; case E1_14: switch (code) { case 0x77: state = E1_14_77; break; default: state = INIT; } break; case E1_14_77: switch (code) { case 0xE1: state = E1_14_77_E1; break; default: state = INIT; } break; case E1_14_77_E1: switch (code) { case 0xF0: state = E1_14_77_E1_F0; break; default: state = INIT; } break; case E1_14_77_E1_F0: switch (code) { case 0x14: state = E1_14_77_E1_F0_14; break; default: state = INIT; } break; case E1_14_77_E1_F0_14: switch (code) { case 0xF0: state = E1_14_77_E1_F0_14_F0; break; default: state = INIT; } break; case E1_14_77_E1_F0_14_F0: switch (code) { case 0x77: matrix_make(PAUSE); state = INIT; break; default: state = INIT; } break; // Following are states of Control'd Pause case E0_7E: if (code == 0xE0) state = E0_7E_E0; else state = INIT; break; case E0_7E_E0: if (code == 0xF0) state = E0_7E_E0_F0; else state = INIT; break; case E0_7E_E0_F0: if (code == 0x7E) matrix_make(PAUSE); state = INIT; break; default: state = INIT; } } // TODO: request RESEND when error occurs? /* if (PS2_IS_FAILED(ps2_error)) { uint8_t ret = ps2_host_send(PS2_RESEND); xprintf("Resend: %02X\n", ret); } */ return 1; }
uint8_t matrix_scan(void) { // scan code reading states static enum { RESET, RESET_RESPONSE, KBD_ID0, KBD_ID1, CONFIG, READY, F0, } state = RESET; is_modified = false; uint8_t code; if ((code = ps2_host_recv())) { debug("r"); debug_hex(code); debug(" "); } switch (state) { case RESET: debug("wFF "); if (ps2_host_send(0xFF) == 0xFA) { debug("[ack]\nRESET_RESPONSE: "); state = RESET_RESPONSE; } break; case RESET_RESPONSE: if (code == 0xAA) { debug("[ok]\nKBD_ID: "); state = KBD_ID0; } else if (code) { debug("err\nRESET: "); state = RESET; } break; // after reset receive keyboad ID(2 bytes) case KBD_ID0: if (code) { state = KBD_ID1; } break; case KBD_ID1: if (code) { debug("\nCONFIG: "); state = CONFIG; } break; case CONFIG: debug("wF8 "); if (ps2_host_send(0xF8) == 0xFA) { debug("[ack]\nREADY\n"); state = READY; } break; case READY: switch (code) { case 0x00: break; case 0xF0: state = F0; debug(" "); break; default: // normal key make if (code < 0x88) { matrix_make(code); } else { debug("unexpected scan code at READY: "); debug_hex(code); debug("\n"); } state = READY; debug("\n"); } break; case F0: // Break code switch (code) { case 0x00: break; default: if (code < 0x88) { matrix_break(code); } else { debug("unexpected scan code at F0: "); debug_hex(code); debug("\n"); } state = READY; debug("\n"); } break; } return 1; }
/* scan all key states on matrix */ uint8_t matrix_scan(void) { _delay_ms(5); //next_kbd_set_leds(false, false); NEXT_KBD_LED1_OFF; is_modified = false; if (!NEXT_KBD_PWR_READ) { matrix_make(NEXT_KBD_PWR_KEYCODE); power_state = 1; if (is_modified) { dprintf("Power state 1\n"); } } else { matrix_break(NEXT_KBD_PWR_KEYCODE); power_state = 0; if (is_modified) { dprintf("Power state 0\n"); } } uint32_t resp = (next_kbd_recv()); if (resp == NEXT_KBD_KMBUS_IDLE) { return 0; } NEXT_KBD_LED1_ON; #ifdef NEXT_KBD_SHIFT_FLASH_LEDS next_kbd_set_leds( NEXT_KBD_PRESSED_SHIFT_LEFT(resp) ? true : false, NEXT_KBD_PRESSED_SHIFT_RGHT(resp) ? true : false ); #endif dprintf("[ r=%04lX keycode=%02X pressed=%X CTRL=%X SHIFT_LEFT=%X SHIFT_RGHT=%X CMD_LEFT=%X CMD_RGHT=%X ALT_LEFT=%X ALT_RGHT=%X ]\n", \ resp, \ NEXT_KBD_KEYCODE(resp), \ NEXT_KBD_PRESSED_KEYCODE(resp), \ NEXT_KBD_PRESSED_CONTROL(resp), \ NEXT_KBD_PRESSED_SHIFT_LEFT(resp), \ NEXT_KBD_PRESSED_SHIFT_RGHT(resp), \ NEXT_KBD_PRESSED_CMD_LEFT(resp), \ NEXT_KBD_PRESSED_CMD_RGHT(resp), \ NEXT_KBD_PRESSED_ALT_LEFT(resp), \ NEXT_KBD_PRESSED_ALT_RGHT(resp) \ ); // Modifier keys don't return keycode; have to check the upper bits NEXT_KBD_MAKE_OR_BREAK(ALT_RGHT, 0x51); NEXT_KBD_MAKE_OR_BREAK(ALT_LEFT, 0x52); NEXT_KBD_MAKE_OR_BREAK(CMD_RGHT, 0x53); NEXT_KBD_MAKE_OR_BREAK(CMD_LEFT, 0x54); NEXT_KBD_MAKE_OR_BREAK(SHIFT_RGHT, 0x55); NEXT_KBD_MAKE_OR_BREAK(SHIFT_LEFT, 0x56); NEXT_KBD_MAKE_OR_BREAK(CONTROL, 0x57); NEXT_KBD_MAKE_OR_BREAK(KEYCODE, NEXT_KBD_KEYCODE(resp)); return 1; }
/* * PS/2 Scan Code Set 2: Exceptional Handling * * There are several keys to be handled exceptionally. * The scan code for these keys are varied or prefix/postfix'd * depending on modifier key state. * * References: * http://www.microsoft.com/whdc/archive/scancode.mspx * http://download.microsoft.com/download/1/6/1/161ba512-40e2-4cc9-843a-923143f3456c/scancode.doc * * * Insert, Delete, Home, End, PageUp, PageDown, Up, Down, Right, Left: * Num Lock: off * modifiers | make | break * ----------+---------------------------+---------------------- * Ohter | <make> | <break> * LShift | E0 F0 12 <make> | <break> E0 12 * RShift | E0 F0 59 <make> | <break> E0 59 * L+RShift | E0 F0 12 E0 F0 59 <make> | <break> E0 59 E0 12 * * Num Lock: on * modifiers | make | break * ----------+---------------------------+---------------------- * Other | E0 12 <make> | <break> E0 F0 12 * Shift'd | <make> | <break> * * Handling: ignore these prefix/postfix codes * * * Keypad-/: * modifiers | make | break * ----------+---------------------------+---------------------- * Ohter | <make> | <break> * LShift | E0 F0 12 <make> | <break> E0 12 * RShift | E0 F0 59 <make> | <break> E0 59 * L+RShift | E0 F0 12 E0 F0 59 <make> | <break> E0 59 E0 12 * * Handling: ignore these prefix/postfix codes * * * PrintScreen: * With hoding down modifiers, the scan code is sent as following: * * modifiers | make | break * ----------+--------------+----------------------------------- * Other | E0 12 E0 7C | E0 F0 7C E0 F0 12 * Shift'd | E0 7C | E0 F0 7C * Control'd | E0 7C | E0 F0 7C * Alt'd | 84 | F0 84 * * Handling: ignore prefix/postfix codes and treat both scan code * E0 7C and 84 as PrintScreen. * * Pause: * With hoding down modifiers, the scan code is sent as following: * * modifiers | make(no break code) * ----------+-------------------------------------------------- * no mods | E1 14 77 E1 F0 14 F0 77 * Control'd | E0 7E E0 F0 7E * * Handling: treat these two code sequence as Pause * */ uint8_t matrix_scan(void) { static enum { INIT, F0, E0, E0_F0, // states for Pause/Break E1, E1_14, E1_14_77, E1_14_77_E1, E1_14_77_E1_F0, E1_14_77_E1_F0_14, E1_14_77_E1_F0_14_F0, } state = INIT; is_modified = false; // Pause/Break off(PS/2 has no break for this key) if (matrix_is_on(ROW(PAUSE), COL(PAUSE))) { matrix_break(PAUSE); } uint8_t code; while ((code = ps2_host_recv())) { switch (state) { case INIT: switch (code) { case 0xE0: // 2byte make state = E0; break; case 0xF0: // break code state = F0; break; case 0xE1: // Pause/Break state = E1; break; case 0x83: // F8 matrix_make(F8); state = INIT; break; case 0x84: // PrintScreen matrix_make(PRINT_SCREEN); state = INIT; break; default: // normal key make if (code < 0x80) { matrix_make(code); } else { debug("unexpected scan code at INIT: "); debug_hex(code); debug("\n"); } state = INIT; } break; case E0: switch (code) { case 0x12: // postfix/postfix code for exceptional keys case 0x59: // postfix/postfix code for exceptional keys // ignore state = INIT; break; case 0x7E: // former part of Control-Pause[E0 7E E0 F0 7E] matrix_make(PAUSE); state = INIT; break; case 0xF0: // E0 break state = E0_F0; break; default: // E0 make if (code < 0x80) { matrix_make(code|0x80); } else { debug("unexpected scan code at E0: "); debug_hex(code); debug("\n"); } state = INIT; } break; case F0: switch (code) { case 0x83: matrix_break(F8); state = INIT; break; case 0x84: matrix_break(PRINT_SCREEN); state = INIT; break; default: if (code < 0x80) { matrix_break(code); } else { debug("unexpected scan code at F0: "); debug_hex(code); debug("\n"); } state = INIT; } break; case E0_F0: // E0 break switch (code) { case 0x12: // postfix/postfix code for exceptional keys case 0x59: // postfix/postfix code for exceptional keys case 0x7E: // latter part of Control-Pause[E0 7E E0 F0 7E] // ignore state = INIT; break; default: if (code < 0x80) { matrix_break(code|0x80); } else { debug("unexpected scan code at E0_F0: "); debug_hex(code); debug("\n"); } state = INIT; } break; /* Pause */ case E1: switch (code) { case 0x14: state = E1_14; break; default: state = INIT; } break; case E1_14: switch (code) { case 0x77: state = E1_14_77; break; default: state = INIT; } break; case E1_14_77: switch (code) { case 0xE1: state = E1_14_77_E1; break; default: state = INIT; } break; case E1_14_77_E1: switch (code) { case 0xF0: state = E1_14_77_E1_F0; break; default: state = INIT; } break; case E1_14_77_E1_F0: switch (code) { case 0x14: state = E1_14_77_E1_F0_14; break; default: state = INIT; } break; case E1_14_77_E1_F0_14: switch (code) { case 0xF0: state = E1_14_77_E1_F0_14_F0; break; default: state = INIT; } break; case E1_14_77_E1_F0_14_F0: switch (code) { case 0x77: matrix_make(PAUSE); state = INIT; break; default: state = INIT; } break; default: state = INIT; } phex(code); } return 1; }
uint8_t matrix_scan(void) { // scan code reading states static enum { INIT, E0, E0_2A, E0_2A_E0, E0_B7, E0_B7_E0, // print screen E1, E1_1D, E1_1D_45, E1_1D_45_E1, E1_1D_45_E1_9D, // pause } state = INIT; is_modified = false; // 'pseudo break code' hack if (matrix_is_on(ROW(PAUSE), COL(PAUSE))) { matrix_break(PAUSE); } uint8_t code = xt_host_recv(); switch (state) { case INIT: switch (code) { case 0xE0: state = E0; break; case 0xE1: state = E1; break; default: // normal key make if (code < 0x80 && code != 0x00) { xprintf("make: %X\r\n", code); matrix_make(code); } else if (code > 0x80 && code < 0xFF && code != 0x00) { xprintf("break %X\r\n", code); matrix_break(code - 0x80); } state = INIT; } break; case E0: // E0-Prefixed switch (code) { //move these codes to unused places on the matrix case 0x2A: state = E0_2A; break; case 0xB7: state = E0_B7; break; default: if (code < 0x80 && code != 0x00) { matrix_make(move_codes(code)); } else if (code > 0x80 && code < 0xFF && code != 0x00) { matrix_break(move_codes(code - 0x80)); } state = INIT; } break; case E0_2A: if(code == 0xE0) state = E0_2A_E0; else state = INIT; break; case E0_2A_E0: if(code == 0x37) matrix_make(PRINT_SCREEN); else state = INIT; break; case E0_B7: if(code == 0xE0) state = E0_B7; else state = INIT; break; case E0_B7_E0: if(code == 0xAA) matrix_break(PRINT_SCREEN); else state = INIT; break; case E1: if (code == 0x1D) state = E1_1D; else state = INIT; break; case E1_1D: if(code == 0x45) state = E1_1D_45; else state = INIT; break; case E1_1D_45: if(code == 0xE1) state = E1_1D_45_E1; else state = INIT; break; case E1_1D_45_E1: if(code == 0x9D) state = E1_1D_45_E1_9D; else state = INIT; break; case E1_1D_45_E1_9D: if(code == 0xC5) matrix_make(PAUSE); else state = INIT; break; default: state = INIT; } return 1; }