home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL004 < prev    next >
Encoding:
Text File  |  1987-03-02  |  10.6 KB  |  638 lines
  1. /*
  2.  * DBOT Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : db
  6.  *      File    : edb.c
  7.  *      Remarks : The data base editor.
  8.  *      Usage   : make edb
  9.  */
  10.  
  11. #include <stdio.h>
  12. #include <ctype.h>
  13. #include <signal.h>
  14. #include "../h/rccl.h"
  15. #include "../h/umac.h"
  16.  
  17. #define BEGIN   0
  18. #define RW      2
  19. #define BUFS    1024
  20.  
  21. static TRSF cht;
  22. static TRSF_PTR ch = &cht;
  23. static TRSF pht;
  24. static TRSF_PTR ph = &pht;
  25. static char readn[80];
  26. static errset;
  27.  
  28. /*
  29.  * works on a copy of the file
  30.  * if exit by 'q' copy new into old, keep old
  31.  * if exit by 'q!' copy the copy into original
  32.  */
  33.  
  34. main(argc, argv)
  35. int argc;
  36. char **argv;
  37. {
  38.     char *strcat(), *strcpy(), *sprintf();
  39.     int intr();
  40.     char st[20], sto[20], sti[20];
  41.     int fddb, fddbo, fddbi;
  42.     int q, action, saved = YES;
  43.     char trname[80];
  44.     char *unitn = "UNIT";
  45.     TRSF tr;
  46.  
  47.     if (argc != 2) {
  48.         fprintf(stderr, "usage : edbt file\n");
  49.         exit(1);
  50.     }
  51.  (void) strcpy(st, argv[1]);
  52.     if ((fddb = open(st, RW)) < 0) {
  53.         printf("can't open %s, create ?", st);
  54.         QUERY(q);
  55.         if (q == 'y') {
  56.             if ((fddb = makedb(st)) < 0) {
  57.                 exit(14);
  58.             }
  59.             if ((fddb = open(st, RW)) < 0) {
  60.                 exit(15);
  61.             }
  62.         }
  63.         else {
  64.             exit(0);
  65.         }
  66.     }
  67.  
  68.  (void) signal(SIGINT, intr);
  69.  (void) strcpy(sto, st);
  70.  (void) strcat(sto, ".old");
  71.     fddbo = make(sto);
  72.     copy(fddbo, fddb);
  73.     if ((fddbo = open(sto, RW)) < 0) {
  74.         fprintf(stderr, "can't open %s\n", sto);
  75.         exit(2);
  76.     }
  77.     printf("transform data base editor V1.0\n");
  78.     tr.name = trname;
  79.  (void) strcpy(trname, unitn);
  80.     Assigntr(&tr, unitr);
  81.     for (; ; ) {
  82.         Assigntr(ch, unitr);
  83.         while ((action = parse()) == 'e' || action == ' ') {
  84.         }
  85.         switch (action) {
  86.         case 'x' :
  87.             copy(fddbo, fddb);
  88.             exit(0);
  89.  
  90.         case 'q' :
  91.             if (!saved) {
  92.                 printf("loose %s ?", tr.name);
  93.                 QUERY(q);
  94.                 if (q == 'n') {
  95.                     break;
  96.                 }
  97.             }
  98.          (void) sprintf(sti, "%d", getpid());
  99.          (void) strcat(sti, ".edb");
  100.             if (compactdb(sto) < 0) {
  101.                 fprintf(stderr, "compact error\n");
  102.                 exit(8);
  103.             }
  104.             fddbi = make(sti);
  105.             copy(fddbi, fddb);
  106.             fddb = make(st);
  107.             copy(fddb, fddbo);
  108.             fddbo = make(sto);
  109.             copy(fddbo, fddbi);
  110.             if (unlink(sti) < 0) {
  111.                 fprintf(stderr, "could'nt unlink\n");
  112.             }
  113.             exit(0);
  114.  
  115.         case 'd' :
  116.             if (dumpdb(fddbo, NO) < 0) {
  117.                 exit(21);
  118.             }
  119.             break;
  120.  
  121.         case 'D' :
  122.             if (dumpdb(fddbo, YES) < 0) {
  123.                 exit(22);
  124.             }
  125.             break;
  126.  
  127.         case 'u' :
  128.             if (!saved) {
  129.                 printf("loose %s ?", tr.name);
  130.                 QUERY(q);
  131.                 if (q == 'n') {
  132.                     break;
  133.                 }
  134.             }
  135.             if (strcmp(readn, "UNIT") == 0) {
  136.              (void) strcpy(tr.name, unitn);
  137.                 Assigntr(&tr, unitr);
  138.                 break;
  139.             }
  140.             tr.name = readn;
  141.             switch (gettr(&tr, fddbo)) {
  142.             case -1 :
  143.                 exit(24);
  144.  
  145.             case -2 :
  146.                 tr.name = trname;
  147.                 break;
  148.  
  149.             default :
  150.                 tr.name = trname;
  151.              (void) strcpy(tr.name, readn);
  152.                 break;
  153.             }
  154.             saved = YES;
  155.             break;
  156.  
  157.         case 's' :
  158.             if ((q = savetr(&tr, fddbo)) < 0) {
  159.                 exit(23);
  160.             }
  161.             if (q == 0) {
  162.                 saved = YES;
  163.             }
  164.             break;
  165.  
  166.         case 'n' :
  167.          (void) strcpy(tr.name, readn);
  168.             printf("renamed %s\n", tr.name);
  169.             saved = NO;
  170.             break;
  171.  
  172.         case ':' :
  173.             printrn(&tr, stdout);
  174.             break;
  175.  
  176.         case 'r' :
  177.             printf("remove %s ?", readn);
  178.             QUERY(q);
  179.             if (q == 'y') {
  180.                 if (remtr(readn, fddbo) == -1) {
  181.                     exit(25);
  182.                 }
  183.             }
  184.             break;
  185.  
  186.         case 'y' :
  187.             saved = NO;
  188.             Trmultinp(&tr, ch);
  189.             break;
  190.  
  191.         case 'p' :
  192.             saved = NO;
  193.             Taketrsl(&tr, ph);
  194.             break;
  195.  
  196.         case 'P' :
  197.             saved = NO;
  198.             Takerot(&tr, ph);
  199.             break;
  200.  
  201.         case 'i' :
  202.             saved = NO;
  203.             Invertinp(&tr);
  204.             break;
  205.  
  206.         default :
  207.             break;
  208.         }
  209.     }
  210. }
  211.  
  212.  
  213.  
  214. static char coml[80], *c;
  215.  
  216.  
  217. static parse()  /*##*/
  218. {
  219.     double getreal();
  220.     double x1, x2, x3, x4, x5, x6;
  221.  
  222.     errset = NO;
  223.     putchar('?');
  224.  (void) gets(coml);
  225.     if (strlen(coml) == 0) {
  226.         return(' ');
  227.     }
  228.  
  229.     for (c = coml; *c != '\0';) {
  230.         while (*c != '\0' && isspace(*c)) {
  231.             ++c;
  232.         }
  233.         switch (*c) {
  234.         case 'q' :
  235.             ++c;
  236.             if (*c == '!') {
  237.                 if (*++c != '\0') {
  238.                     err();
  239.                     return('e');
  240.                 }
  241.                 return('x');
  242.             }
  243.             else {
  244.                 if (*c != '\0') {
  245.                     err();
  246.                     return('e');
  247.                 }
  248.                 return('q');
  249.             }
  250.  
  251.         case 'r' :
  252.             while (isspace(*++c))
  253.                 ;
  254.          (void) strcpy(readn, c);
  255.             return('r');
  256.  
  257.         case 'n' :
  258.             while (isspace(*++c))
  259.                 ;
  260.          (void) strcpy(readn, c);
  261.             if (strlen(readn) == 0) {
  262.                 err();
  263.                 return('e');
  264.             }
  265.             return('n');
  266.  
  267.         case ':' :
  268.             if (*++c != '\0') {
  269.                 err();
  270.                 return('e');
  271.             }
  272.             return(':');
  273.  
  274.         case 's' :
  275.             if (*++c != '\0') {
  276.                 err();
  277.                 return('e');
  278.             }
  279.             return('s');
  280.  
  281.         case 'd' :
  282.             ++c;
  283.             if (*c == 'v') {
  284.                 if (*++c != '\0') {
  285.                     err();
  286.                     return('e');
  287.                 }
  288.                 return('D');
  289.             }
  290.             else {
  291.                 if (*c != '\0') {
  292.                     err();
  293.                     return('e');
  294.                 }
  295.                 return('d');
  296.             }
  297.  
  298.         case 'u' :
  299.             while (isspace(*++c))
  300.                 ;
  301.          (void) strcpy(readn, c);
  302.             if (strlen(readn) == 0) {
  303.                 err();
  304.                 return('e');
  305.             }
  306.             return('u');
  307.  
  308.         case 'i':
  309.             if (*++c != '\0') {
  310.                 err();
  311.                 return('e');
  312.             }
  313.             return('i');
  314.  
  315.         case 'p' :
  316.             switch(*++c) {
  317.             case 't' :
  318.                 x1 = getreal();
  319.                 x2 = getreal();
  320.                 x3 = getreal();
  321.                 if (!errset) {
  322.                     Trsl(ph, x1, x2, x3);
  323.                 }
  324.                 else {
  325.                     return('e');
  326.                 }
  327.                 return('p');
  328.  
  329.             case 'X' :
  330.                 Rot(ph, xunit, getreal());
  331.                 return('P');
  332.  
  333.             case 'Y' :
  334.                 Rot(ph, yunit, getreal());
  335.                 return('P');
  336.  
  337.             case 'Z' :
  338.                 Rot(ph, zunit, getreal());
  339.                 return('P');
  340.  
  341.             case 'a' :
  342.                 x1 = getreal();
  343.                 x2 = getreal();
  344.                 x3 = getreal();
  345.                 x4 = getreal();
  346.                 x5 = getreal();
  347.                 x6 = getreal();
  348.                 if (!errset) {
  349.                     Vao(ph, x1, x2, x3, x4, x5, x6);
  350.                 }
  351.                 else {
  352.                     return('e');
  353.                 }
  354.                 return('P');
  355.  
  356.             case 'e' :
  357.                 x1 = getreal();
  358.                 x2 = getreal();
  359.                 x3 = getreal();
  360.                 if (!errset) {
  361.                     Eul(ph, x1, x2, x3);
  362.                 }
  363.                 else {
  364.                     return('e');
  365.                 }
  366.                 return('P');
  367.  
  368.             case 'r' :
  369.                 x1 = getreal();
  370.                 x2 = getreal();
  371.                 x3 = getreal();
  372.                 if (!errset) {
  373.                     Rpy(ph, x1, x2, x3);
  374.                 }
  375.                 else {
  376.                     return('e');
  377.                 }
  378.                 return('P');
  379.  
  380.             default :
  381.                 err();
  382.                 break;
  383.             }
  384.             break;
  385.  
  386.         case 'm' :
  387.             switch(*++c) {
  388.             case 't' :
  389.                 x1 = getreal();
  390.                 x2 = getreal();
  391.                 x3 = getreal();
  392.                 if (!errset) {
  393.                     Trslm(ch, x1, x2, x3);
  394.                 }
  395.                 else {
  396.                     return('e');
  397.                 }
  398.                 break;
  399.  
  400.             case 'X' :
  401.                 Rotm(ch, xunit, getreal());
  402.                 break;
  403.  
  404.             case 'Y' :
  405.                 Rotm(ch, yunit, getreal());
  406.                 break;
  407.  
  408.             case 'Z' :
  409.                 Rotm(ch, zunit, getreal());
  410.                 break;
  411.  
  412.             case 'a' :
  413.                 x1 = getreal();
  414.                 x2 = getreal();
  415.                 x3 = getreal();
  416.                 x4 = getreal();
  417.                 x5 = getreal();
  418.                 x6 = getreal();
  419.                 if (!errset) {
  420.                     Vaom(ch, x1, x2, x3, x4, x5, x6);
  421.                 }
  422.                 else {
  423.                     return('e');
  424.                 }
  425.                 break;
  426.  
  427.             case 'e' :
  428.                 x1 = getreal();
  429.                 x2 = getreal();
  430.                 x3 = getreal();
  431.                 if (!errset) {
  432.                     Eulm(ch, x1, x2, x3);
  433.                 }
  434.                 else {
  435.                     return('e');
  436.                 }
  437.                 break;
  438.  
  439.             case 'r' :
  440.                 x1 = getreal();
  441.                 x2 = getreal();
  442.                 x3 = getreal();
  443.                 if (!errset) {
  444.                     Rpym(ch, x1, x2, x3);
  445.                 }
  446.                 else {
  447.                     return('e');
  448.                 }
  449.                 break;
  450.  
  451.             default :
  452.                 err();
  453.                 return('e');
  454.             }
  455.             break;
  456.  
  457.         case '\0':
  458.             break;
  459.  
  460.         default :
  461.             err();
  462.             return('e');
  463.         }
  464.     }
  465.     return('y');
  466. }
  467.  
  468.  
  469. /*
  470.  *                  -------        -< d >-
  471.  *                \|       |     \|       |
  472.  *                 --< d >---< . >---------
  473.  *       -< + >-  |        |              |
  474.  *      |       | |         --------------|/
  475.  *   >--------------< . >----< d >-------------->
  476.  *      |       |          |\      |
  477.  *       -< - >-            -------
  478.  */
  479.  
  480. static double getreal()  /*##*/
  481. {
  482.     char *f;
  483.     double v;
  484.  
  485.     ++c;
  486.     while (isspace(*c)) {
  487.         ++c;
  488.     }
  489.     f = c;
  490.     switch (*c) {
  491.     case '\0' :
  492.         err();
  493.         return(0.);
  494.  
  495.     case '+' :
  496.         ++c;
  497.         break;
  498.  
  499.     case '-' :
  500.         ++c;
  501.         break;
  502.     }
  503.     switch (*c) {
  504.     case '\0' :
  505.         err();
  506.         return(0.);
  507.  
  508.     case '.' :
  509.         ++c;
  510.         if (!isdigit(*c)) {
  511.             err();
  512.             return(0.);
  513.         }
  514.         while (isdigit(*c)) {
  515.             ++c;
  516.         }
  517.         break;
  518.  
  519.     default :
  520.         if (!isdigit(*c)) {
  521.             err();
  522.             return(0.);
  523.         }
  524.  
  525.         while (isdigit(*c)) {
  526.             ++c;
  527.         }
  528.         if (*c == '.') {
  529.             ++c;
  530.             while(isdigit(*c)) {
  531.                 ++c;
  532.             }
  533.         }
  534.         break;
  535.     }
  536.     v = atof(f);
  537.     if (FABS(v) > 10000) {
  538.         err();
  539.         v = 0.;
  540.     }
  541.     return(v);
  542. }
  543.  
  544.  
  545.  
  546. static err() /*##*/
  547. {
  548.     int n;
  549.  
  550.     if (errset) {
  551.         return;
  552.     }
  553.     errset = YES;
  554.     *c = '\0';
  555.     n = strlen(coml) + 1;
  556.     while (n--) {
  557.         putchar(' ');
  558.     }
  559.     putchar('^');
  560.     putchar('\n');
  561. }
  562.  
  563.  
  564. static copy(t, s)  /*##*/
  565. int t, s;
  566. {
  567.     char bd[BUFS];
  568.     int n;
  569.     long lseek();
  570.  
  571.     if (lseek(s, 0L, BEGIN) < 0) {
  572.         fprintf(stderr, "can't rewind data base file\n");
  573.         exit(10);
  574.     }
  575.     if (lseek(t, 0L, BEGIN) < 0) {
  576.         fprintf(stderr, "can't rewind data base file\n");
  577.         exit(11);
  578.     }
  579.     while ((n = read(s, bd, BUFS)) > 0) {
  580.         if (write(t, bd, n) < 0) {
  581.             fprintf(stderr, "can't duplicate data base file\n");
  582.             exit(12);
  583.         }
  584.     }
  585. }
  586.  
  587.  
  588. static make(n)  /*##*/
  589. char *n;
  590. {
  591.     int f;
  592.  
  593.     if ((f = creat(n, 0644)) < 0) {
  594.         fprintf(stderr, "can't creat data base file %s\n", n);
  595.         exit(20);
  596.     }
  597.     if ((f = open(n, RW)) < 0) {
  598.         fprintf(stderr, "open error on data base file %s\n", n);
  599.         exit(21);
  600.     }
  601.     return (f);
  602. }
  603.  
  604.  
  605. static char help[] =
  606. "\
  607. These commands are executed one per line:\n\
  608. \tq\t\tquit and save file\n\
  609. \tq!\t\tquit and do not save\n\
  610. \td[v]\t\tdump data base [verbose]\n\
  611. \tu <name>\tuse transform 'name'\n\
  612. \ts\t\tsave active transform\n\
  613. \tn <name>\trename active transform\n\
  614. \t:\t\tshow active transform\n\
  615. \tr <name>\tremove transform 'name' from file\n\
  616. \ti\t\tinvert active transform\n\
  617. \tpt x y z\tpatch a translation x y z\n\
  618. \tp<X/Y/Z> a\tpatch a rotation a around X, Y, or Z axis\n\
  619. \tpa x y z x y z\tpatch a rotation defined by a and o vectors\n\
  620. \tpe phi the psi\tpatch a rotation from Euler angles\n\
  621. \tpr phi the psi\tpatch a rotation from roll pitch and yaw angles\n\
  622. These commands are cumulative:\n\
  623. \tmt x y z\tmultiply by translation x y z\n\
  624. \tm<X/Y/Z> a\tmultiply by rotation a around X, Y, or Z axis\n\
  625. \tma x y z x y z\tmultiply by rotation defined by a and o vectors\n\
  626. \tme phi the psi\tmultiply by rotation from Euler angles\n\
  627. \tmr phi the psi\tmultiply by rotation from roll pitch and yaw angles\n\
  628. ";
  629.  
  630.  
  631. static intr()  /*##*/
  632. {
  633.     signal(SIGINT, intr);
  634.     printf("commands:\n");
  635.     printf("%s", help);
  636.     *coml = '\0';
  637. }
  638.