home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL034 < prev    next >
Encoding:
Text File  |  1987-03-02  |  10.8 KB  |  557 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : trans.c
  7.  *      Remarks : All the vector and transform utility functions.
  8.  *      Usage   : part of the library
  9.  */
  10.  
  11. /*LINTLIBRARY*/
  12.  
  13.  
  14. #include "../h/rccl.h"
  15. #include "../h/umac.h"
  16.  
  17. /*
  18.  * make a copy of a vector
  19.  */
  20.  
  21. VECT_PTR assignvect(r, v) /*::*/
  22. register VECT_PTR r, v;
  23. {
  24.     r->x = v->x;
  25.     r->y = v->y;
  26.     r->z = v->z;
  27.     return(r);
  28. }
  29.  
  30.  
  31. /*
  32.  * computes dot product of two vectors
  33.  */
  34.  
  35. real dot(u, v) /*::*/
  36. register VECT_PTR u, v;
  37. {
  38.     return(u->x * v->x + u->y * v->y + u->z * v->z);
  39. }
  40.  
  41.  
  42. /*
  43.  * computes the cross product of two vectors
  44.  */
  45.  
  46. VECT_PTR cross(r, u, v) /*::*/
  47. register VECT_PTR r, u, v;
  48. {
  49.     r->x = u->y * v->z - u->z * v->y;
  50.     r->y = u->z * v->x - u->x * v->z;
  51.     r->z = u->x * v->y - u->y * v->x;
  52.     return(r);
  53. }
  54.  
  55.  
  56. /*
  57.  * brings the magnitude of a vector to unity
  58.  */
  59.  
  60. VECT_PTR unit(v, u) /*::*/
  61. register VECT_PTR v, u;
  62. {
  63.     real m;
  64.  
  65.     m = sqrt(dot(u, u));
  66.     if (m < SMALL) {
  67.         giveup("cannot unit vector", YES);
  68.     }
  69.     v->x = u->x / m;
  70.     v->y = u->y / m;
  71.     v->z = u->z / m;
  72.     return(v);
  73. }
  74.  
  75.  
  76. /*
  77.  * assigns t (rhs) transform to r (lhs)  transform
  78.  */
  79.  
  80. TRSF_PTR assigntr(r, t) /*::*/
  81. register TRSF_PTR r, t;
  82. {
  83.     r->n.x = t->n.x;
  84.     r->o.x = t->o.x;
  85.     r->a.x = t->a.x;
  86.     r->p.x = t->p.x;
  87.     r->n.y = t->n.y;
  88.     r->o.y = t->o.y;
  89.     r->a.y = t->a.y;
  90.     r->p.y = t->p.y;
  91.     r->n.z = t->n.z;
  92.     r->o.z = t->o.z;
  93.     r->a.z = t->a.z;
  94.     r->p.z = t->p.z;
  95.     return(r);
  96. }
  97.  
  98.  
  99. /*
  100.  * assigns translation part of t (rhs) to  r (lhs)
  101.  */
  102.  
  103. TRSF_PTR taketrsl(r, t) /*::*/
  104. register TRSF_PTR r, t;
  105. {
  106.     r->p.x = t->p.x;
  107.     r->p.y = t->p.y;
  108.     r->p.z = t->p.z;
  109.     return(r);
  110. }
  111.  
  112. /*
  113.  * assigns rotational part of t (rhs) to  r (lhs)
  114.  */
  115.  
  116. TRSF_PTR takerot(r, t) /*::*/
  117. register TRSF_PTR r, t;
  118. {
  119.     r->n.x = t->n.x;
  120.     r->o.x = t->o.x;
  121.     r->a.x = t->a.x;
  122.     r->n.y = t->n.y;
  123.     r->o.y = t->o.y;
  124.     r->a.y = t->a.y;
  125.     r->n.z = t->n.z;
  126.     r->o.z = t->o.z;
  127.     r->a.z = t->a.z;
  128.     return(r);
  129. }
  130.  
  131.  
  132.  
  133. /*
  134.  * returns in r matrix the product of t by u
  135.  * r t u should be different matrices
  136.  */
  137.  
  138. TRSF_PTR trmult(r, t, u) /*::*/
  139. register TRSF_PTR r, t, u;
  140. {
  141.     r->n.x = t->n.x * u->n.x + t->o.x * u->n.y + t->a.x * u->n.z;
  142.     r->n.y = t->n.y * u->n.x + t->o.y * u->n.y + t->a.y * u->n.z;
  143.     r->n.z = t->n.z * u->n.x + t->o.z * u->n.y + t->a.z * u->n.z;
  144.     r->o.x = t->n.x * u->o.x + t->o.x * u->o.y + t->a.x * u->o.z;
  145.     r->o.y = t->n.y * u->o.x + t->o.y * u->o.y + t->a.y * u->o.z;
  146.     r->o.z = t->n.z * u->o.x + t->o.z * u->o.y + t->a.z * u->o.z;
  147.     r->a.x = t->n.x * u->a.x + t->o.x * u->a.y + t->a.x * u->a.z;
  148.     r->a.y = t->n.y * u->a.x + t->o.y * u->a.y + t->a.y * u->a.z;
  149.     r->a.z = t->n.z * u->a.x + t->o.z * u->a.y + t->a.z * u->a.z;
  150.     r->p.x = t->n.x * u->p.x + t->o.x * u->p.y + t->a.x * u->p.z + t->p.x;
  151.     r->p.y = t->n.y * u->p.x + t->o.y * u->p.y + t->a.y * u->p.z + t->p.y;
  152.     r->p.z = t->n.z * u->p.x + t->o.z * u->p.y + t->a.z * u->p.z + t->p.z;
  153.     return(r);
  154. }
  155.  
  156.  
  157. /*
  158.  * matrix mult in place
  159.  */
  160.  
  161. TRSF_PTR trmultinp(r, m) /*::*/
  162. register TRSF_PTR r, m;
  163. {
  164.     TRSF temp;
  165.  
  166.     return(trmult(r, assigntr(&temp, r), m));
  167. }
  168.  
  169.  
  170. /*
  171.  * matrix mult by inverse
  172.  */
  173.  
  174. TRSF_PTR trmultinv(r, m) /*::*/
  175. register TRSF_PTR r, m;
  176. {
  177.     TRSF temp1, temp2;
  178.  
  179.     return(trmult(r, assigntr(&temp2, r), invert(&temp1, m)));
  180. }
  181.  
  182.  
  183. /*
  184.  * computes the inverse of a transform
  185.  */
  186.  
  187. TRSF_PTR invert(i, t)  /*::*/
  188. register TRSF_PTR i, t;
  189. {
  190.     i->n.x = t->n.x;
  191.     i->n.y = t->o.x;
  192.     i->n.z = t->a.x;
  193.     i->o.x = t->n.y;
  194.     i->o.y = t->o.y;
  195.     i->o.z = t->a.y;
  196.     i->a.x = t->n.z;
  197.     i->a.y = t->o.z;
  198.     i->a.z = t->a.z;
  199.  
  200.     i->p.x = - (t->p.x * t->n.x + t->p.y * t->n.y + t->p.z * t->n.z);
  201.     i->p.y = - (t->p.x * t->o.x + t->p.y * t->o.y + t->p.z * t->o.z);
  202.     i->p.z = - (t->p.x * t->a.x + t->p.y * t->a.y + t->p.z * t->a.z);
  203.     return(i);
  204. }
  205.  
  206.  
  207. /*
  208.  * invert in place
  209.  */
  210.  
  211. TRSF_PTR invertinp(t) /*::*/
  212. TRSF_PTR t;
  213. {
  214.     TRSF temp;
  215.  
  216.     return(invert(t, assigntr(&temp, t)));
  217. }
  218.  
  219.  
  220. /*
  221.  * set a pure translation transform
  222.  */
  223.  
  224. TRSF_PTR trsl(t, x, y, z) /*::*/
  225. register TRSF_PTR t;
  226. real x, y, z;
  227. {
  228.     t->p.x = x;
  229.     t->p.y = y;
  230.     t->p.z = z;
  231.     return(t);
  232. }
  233.  
  234.  
  235. /*
  236.  * multiply a transform by a pure translation transform
  237.  */
  238.  
  239. TRSF_PTR trslm(t, x, y, z) /*::*/
  240. register TRSF_PTR t;
  241. real x, y, z;
  242. {
  243.     static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
  244.  
  245.     temp.n.x = temp.o.y = temp.a.z = 1.;
  246.     temp.n.y = temp.n.z = temp.o.x = temp.o.z = temp.a.x = temp.a.y = 0.;
  247.     return(trmultinp(t, trsl(&temp, x, y, z)));
  248. }
  249.  
  250.  
  251. /*
  252.  * set a transform given the a o vectors
  253.  */
  254.  
  255. TRSF_PTR vao(t, ax, ay, az, ox, oy, oz) /*::*/
  256. register TRSF_PTR t;
  257. real ax, ay, az, ox, oy, oz;
  258. {
  259.     t->a.x = ax;
  260.     t->a.y = ay;
  261.     t->a.z = az;
  262.     t->o.x = ox;
  263.     t->o.y = oy;
  264.     t->o.z = oz;
  265.  
  266.  (void) unit(&t->a, &t->a);
  267.  (void) cross(&t->n, &t->o, &t->a);
  268.  (void) cross(&t->o, &t->a, &t->n);
  269.  (void) unit(&t->o, &t->o);
  270.  (void) cross(&t->n, &t->o, &t->a);
  271.     return(t);
  272. }
  273.  
  274.  
  275. /*
  276.  * multiply a transform by a rotation expressed with a o vectors
  277.  */
  278.  
  279. TRSF_PTR vaom(t, ax, ay, az, ox, oy, oz) /*::*/
  280. register TRSF_PTR t;
  281. real ax, ay, az, ox, oy, oz;
  282. {
  283.     static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
  284.  
  285.     temp.p.z = temp.p.y = temp.p.z = 0.;
  286.     return(trmultinp(t, vao(&temp, ax, ay, az, ox, oy, oz)));
  287. }
  288.  
  289.  
  290. /*
  291.  * sets the terms of a transform given a rotation theta around a vector k
  292.  */
  293.  
  294. TRSF_PTR rot(t, k, h) /*::*/
  295. register TRSF_PTR t;
  296. register VECT_PTR k;
  297. real h;
  298. {
  299.     VECT kl;
  300.     real s, c, ver, x, y, z, xv, yv, zv, xs, ys, zs;
  301.  
  302.  (void) unit(&kl, k);
  303.     s = sin(dgtord_m * h);
  304.     ver = 1. - (c = cos(dgtord_m * h));
  305.     xv = ver * (x = kl.x);
  306.     xs = x * s;
  307.     yv = ver * (y = kl.y);
  308.     ys = y * s;
  309.     zv = ver * (z = kl.z);
  310.     zs = z * s;
  311.  
  312.     t->n.x = x * xv + c;
  313.     t->n.y = x * yv + zs;
  314.     t->n.z = x * zv - ys;
  315.  
  316.     t->o.x = y * xv - zs;
  317.     t->o.y = y * yv + c;
  318.     t->o.z = y * zv + xs;
  319.  
  320.     t->a.x = z * xv + ys;
  321.     t->a.y = z * yv - xs;
  322.     t->a.z = z * zv + c;
  323.     return(t);
  324. }
  325.  
  326.  
  327. /*
  328.  * multiply a transform by a rotation about a vector
  329.  */
  330.  
  331. TRSF_PTR rotm(t, k, h) /*::*/
  332. register TRSF_PTR t;
  333. register VECT_PTR k;
  334. real h;
  335. {
  336.     static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
  337.  
  338.     temp.p.z = temp.p.y = temp.p.z = 0.;
  339.     return(trmultinp(t, rot(&temp, k, h)));
  340. }
  341.  
  342.  
  343. /*
  344.  * sets the terms of a transform given the euler angles
  345.  * expressed in degrees
  346.  */
  347.  
  348. TRSF_PTR eul(t, phi, the, psi) /*::*/
  349. register TRSF_PTR t;
  350. real phi, the, psi;
  351. {
  352.     real sphi, sthe, spsi, cphi, cthe, cpsi;
  353.  
  354.     sphi = sin(dgtord_m * phi);
  355.     cphi = cos(dgtord_m * phi);
  356.     sthe = sin(dgtord_m * the);
  357.     cthe = cos(dgtord_m * the);
  358.     spsi = sin(dgtord_m * psi);
  359.     cpsi = cos(dgtord_m * psi);
  360.  
  361.     t->n.x =   cphi * cthe * cpsi - sphi * spsi;
  362.     t->n.y =   sphi * cthe * cpsi + cphi * spsi;
  363.     t->n.z = - sthe * cpsi;
  364.  
  365.     t->o.x = - cphi * cthe * spsi - sphi * cpsi;
  366.     t->o.y = - sphi * cthe * spsi + cphi * cpsi;
  367.     t->o.z =   sthe * spsi;
  368.  
  369.     t->a.x = cphi * sthe;
  370.     t->a.y = sphi * sthe;
  371.     t->a.z = cthe;
  372.     return(t);
  373. }
  374.  
  375.  
  376. /*
  377.  * multiply a transform by a rotation expressed with euler angles
  378.  */
  379.  
  380. TRSF_PTR eulm(t, phi, the, psi) /*::*/
  381. register TRSF_PTR t;
  382. real phi, the, psi;
  383. {
  384.     static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
  385.  
  386.     temp.p.z = temp.p.y = temp.p.z = 0.;
  387.     return(t, trmultinp(t, eul(&temp, phi, the, psi)));
  388. }
  389.  
  390.  
  391. /*
  392.  * returns the euler angles of the rotation part of a transform with
  393.  * euler angles given in degrees
  394.  */
  395.  
  396. noatoeul(phi, the, psi, t) /*::*/
  397. real *phi, *the, *psi; /* pointers to angles */
  398. register TRSF_PTR t;
  399. {
  400.     real sphi, cphi, p;
  401.  
  402.     if (FABS(t->a.y) > SMALL || FABS(t->a.x) > SMALL) {
  403.         *phi = rdtodg_m * (p = atan2(t->a.y, t->a.x));
  404.         sphi = sin(p);
  405.         cphi = cos(p);
  406.         *the = rdtodg_m * atan2(cphi * t->a.x + sphi * t->a.y, t->a.z);
  407.         *psi = rdtodg_m * atan2(- sphi * t->n.x + cphi * t->n.y ,
  408.                     - sphi * t->o.x + cphi * t->o.y);
  409.     }
  410.     else {
  411.         *phi = 0.;
  412.         *the = rdtodg_m * atan2(t->a.x, t->a.z);
  413.         *psi = rdtodg_m * atan2(t->n.y, t->o.y);
  414.     }
  415. }
  416.  
  417.  
  418. /*
  419.  * sets the term of a transform with a rotation expressed with rpy
  420.  * angles in degrees
  421.  */
  422.  
  423. TRSF_PTR rpy(t, phi, the, psi) /*::*/
  424. register TRSF_PTR t;
  425. real phi, the, psi;
  426. {
  427.     real sphi, sthe, spsi, cphi, cthe, cpsi;
  428.  
  429.     sphi = sin(dgtord_m * phi);
  430.     cphi = cos(dgtord_m * phi);
  431.     sthe = sin(dgtord_m * the);
  432.     cthe = cos(dgtord_m * the);
  433.     spsi = sin(dgtord_m * psi);
  434.     cpsi = cos(dgtord_m * psi);
  435.  
  436.     t->n.x = cphi * cthe;
  437.     t->n.y = sphi * cthe;
  438.     t->n.z = - sthe;
  439.     t->o.x = cphi * sthe * spsi - sphi * cpsi;
  440.     t->o.y = sphi * sthe * spsi + cphi * cpsi;
  441.     t->o.z = cthe * spsi;
  442.     t->a.x = cphi * sthe * cpsi + sphi * spsi;
  443.     t->a.y = sphi * sthe * cpsi - cphi * spsi;
  444.     t->a.z = cthe * cpsi;
  445.     return(t);
  446. }
  447.  
  448.  
  449. /*
  450.  * multilpy a transform with a rotation expressed with rpy anlges
  451.  */
  452.  
  453. TRSF_PTR rpym(t, phi, the, psi) /*::*/
  454. register TRSF_PTR t;
  455. real phi, the, psi;
  456. {
  457.     static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
  458.  
  459.     temp.p.z = temp.p.y = temp.p.z = 0.;
  460.     return(t, trmultinp(t, rpy(&temp, phi, the, psi)));
  461. }
  462.  
  463.  
  464. /*
  465.  * computes rpy angles from the n o a vectors of a transform
  466.  */
  467.  
  468. noatorpy(phi, the, psi, t) /*::*/
  469. real *phi, *the, *psi;
  470. register TRSF_PTR t;
  471. {
  472.     real sphi, cphi, nx, ny, p;
  473.  
  474.     nx = t->n.x;
  475.     ny = t->n.y;
  476.     if (FABS(nx) < SMALL && FABS(ny) < SMALL) {
  477.         *phi = 0.;
  478.         *the = rdtodg_m * atan2(- t->n.z, nx);
  479.         *psi = rdtodg_m * atan2(- t->a.y, t->o.y);
  480.     }
  481.     else {
  482.         *phi = rdtodg_m * (p = atan2(ny, nx));
  483.         sphi = sin(p);
  484.         cphi = cos(p);
  485.         *the = rdtodg_m * atan2(- t->n.z, cphi * nx + sphi * ny);
  486.         *psi = rdtodg_m * atan2(sphi * t->a.x - cphi * t->a.y ,
  487.                     cphi * t->o.y - sphi * t->o.x);
  488.     }
  489. }
  490.  
  491.  
  492.  
  493. /*
  494.  * prints out the numerical information
  495.  */
  496.  
  497. printr(t, fp)  /*::*/
  498. TRSF_PTR t;
  499. FILE *fp;
  500. {
  501.     fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
  502.         t->n.x, t->o.x, t->a.x, t->p.x);
  503.     fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
  504.         t->n.y, t->o.y, t->a.y, t->p.y);
  505.     fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
  506.         t->n.z, t->o.z, t->a.z, t->p.z);
  507. }
  508.  
  509.  
  510. /*
  511.  * prints name value and rpy and euler angles of a transform
  512.  */
  513.  
  514. printrn(t, fp) /*::*/
  515. TRSF_PTR t;
  516. FILE *fp;
  517. {
  518.     fprintf(fp, "%s :\n", t->name);
  519.     printr(t, fp);
  520.     printe(t, fp);
  521.     printy(t, fp);
  522. }
  523.  
  524.  
  525. /*
  526.  * prints out the euler representation of a transform
  527.  */
  528.  
  529. printe(e, fp) /*::*/
  530. TRSF_PTR e;
  531. FILE *fp;
  532. {
  533.     real p, t, s;
  534.  
  535.     noatoeul(&p, &t, &s, e);
  536.     fprintf(fp,
  537.     "EUL x:%-7.3f y:%-7.3f z:%-7.3f phi:%-7.3f the:%-7.3f psi:%-7.3f\n",
  538.     e->p.x, e->p.y, e->p.z, p, t, s);
  539. }
  540.  
  541.  
  542. /*
  543.  * prints out the rpy representation of a transform
  544.  */
  545.  
  546. printy(e, fp) /*::*/
  547. TRSF_PTR e;
  548. FILE *fp;
  549. {
  550.     real p, t, s;
  551.  
  552.     noatorpy(&p, &t, &s, e);
  553.     fprintf(fp,
  554.     "RPY x:%-7.3f y:%-7.3f z:%-7.3f phi:%-7.3f the:%-7.3f psi:%-7.3f\n",
  555.     e->p.x, e->p.y, e->p.z, p, t, s);
  556. }
  557.