home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL028 < prev    next >
Encoding:
Text File  |  1987-03-02  |  3.6 KB  |  179 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : diff.c
  7.  *      Remarks : General purpose functions to deal with diff motions
  8.  *                and forces.
  9.  *      Usage   : part of the library
  10.  */
  11.  
  12. /*LINTLIBRARY*/
  13.  
  14. #include "../h/rccl.h"
  15.  
  16. /*
  17.  * make a TRSF out of a DIFF,
  18.  * n o a have to be united and squared (comply accumulates too much error)
  19.  */
  20.  
  21. TRSF_PTR df_to_tr(t, d) /*::*/
  22. register TRSF_PTR t;
  23. register DIFF_PTR d;
  24. {
  25.     t->p.x = d->t.x;
  26.     t->p.y = d->t.y;
  27.     t->p.z = d->t.z;
  28.     t->n.x = t->o.y = t->a.z = 1.;
  29.     t->a.y = -(t->o.z = d->r.x);
  30.     t->n.z = -(t->a.x = d->r.y);
  31.     t->o.x = -(t->n.y = d->r.z);
  32.  
  33.     Unit(&t->a, &t->a);
  34.     Cross(&t->n, &t->o, &t->a);
  35.     Cross(&t->o, &t->a, &t->n);
  36.     Unit(&t->o, &t->o);
  37.     Cross(&t->n, &t->o, &t->a);
  38.  
  39.     return(t);
  40. }
  41.  
  42.  
  43. /*
  44.  * make a DIFF out of a TRSF
  45.  */
  46.  
  47. DIFF_PTR tr_to_df(d, t) /*::*/
  48. register DIFF_PTR d;
  49. register TRSF_PTR t;
  50. {
  51.     d->t.x = t->p.x;
  52.     d->t.y = t->p.y;
  53.     d->t.z = t->p.z;
  54.     d->r.x = t->o.z;
  55.     d->r.y = t->a.x;
  56.     d->r.z = t->n.y;
  57.     return(d);
  58. }
  59.  
  60.  
  61.  
  62. /*
  63.  * force frame transformation
  64.  */
  65.  
  66. FORCE_PTR forcetr(fc, ff, tr) /*::*/
  67. register FORCE_PTR fc, ff;
  68. register TRSF_PTR tr;
  69. {
  70.     VECT k;
  71.  
  72.     /* k = f X p + m */
  73.  
  74.     k.x = ff->f.y * tr->p.z - ff->f.z * tr->p.y + ff->m.x;
  75.     k.y = ff->f.z * tr->p.x - ff->f.x * tr->p.z + ff->m.y;
  76.     k.z = ff->f.x * tr->p.y - ff->f.y * tr->p.x + ff->m.z;
  77.  
  78.     fc->m.x = tr->n.x * k.x + tr->n.y * k.y + tr->n.z * k.z;
  79.     fc->m.y = tr->o.x * k.x + tr->o.y * k.y + tr->o.z * k.z;
  80.     fc->m.z = tr->a.x * k.x + tr->a.y * k.y + tr->a.z * k.z;
  81.  
  82.     fc->f.x = tr->n.x * ff->f.x + tr->n.y * ff->f.y + tr->n.z * ff->f.z;
  83.     fc->f.y = tr->o.x * ff->f.x + tr->o.y * ff->f.y + tr->o.z * ff->f.z;
  84.     fc->f.z = tr->a.x * ff->f.x + tr->a.y * ff->f.y + tr->a.z * ff->f.z;
  85.     return(fc);
  86. }
  87.  
  88.  
  89.  
  90. /*
  91.  * differential transformation
  92.  */
  93.  
  94.  
  95. DIFF_PTR difftr(dc, dd, tr) /*::*/
  96. register DIFF_PTR dc, dd;
  97. register TRSF_PTR tr;
  98. {
  99.     VECT k;
  100.  
  101.     /* k = r X p + d */
  102.  
  103.     k.x = dd->r.y * tr->p.z - dd->r.z * tr->p.y + dd->t.x;
  104.     k.y = dd->r.z * tr->p.x - dd->r.x * tr->p.z + dd->t.y;
  105.     k.z = dd->r.x * tr->p.y - dd->r.y * tr->p.x + dd->t.z;
  106.  
  107.     dc->t.x = tr->n.x * k.x + tr->n.y * k.y + tr->n.z * k.z;
  108.     dc->t.y = tr->o.x * k.x + tr->o.y * k.y + tr->o.z * k.z;
  109.     dc->t.z = tr->a.x * k.x + tr->a.y * k.y + tr->a.z * k.z;
  110.  
  111.     dc->r.x = tr->n.x * dd->r.x + tr->n.y * dd->r.y + tr->n.z * dd->r.z;
  112.     dc->r.y = tr->o.x * dd->r.x + tr->o.y * dd->r.y + tr->o.z * dd->r.z;
  113.     dc->r.z = tr->a.x * dd->r.x + tr->a.y * dd->r.y + tr->a.z * dd->r.z;
  114.     return(dc);
  115. }
  116.  
  117.  
  118. /*
  119.  * copy a DIFF struct
  120.  */
  121.  
  122. DIFF_PTR assigndiff(t, o) /*::*/
  123. register DIFF_PTR t, o;
  124. {
  125.     t->t.x = o->t.x;
  126.     t->t.y = o->t.y;
  127.     t->t.z = o->t.z;
  128.     t->r.x = o->r.x;
  129.     t->r.y = o->r.y;
  130.     t->r.z = o->r.z;
  131.     return(t);
  132. }
  133.  
  134.  
  135. /*
  136.  * copy a FORCE struct
  137.  */
  138.  
  139. FORCE_PTR assignforce(t, o) /*::*/
  140. register FORCE_PTR t, o;
  141. {
  142.     t->f.x = o->f.x;
  143.     t->f.y = o->f.y;
  144.     t->f.z = o->f.z;
  145.     t->m.x = o->m.x;
  146.     t->m.y = o->m.y;
  147.     t->m.z = o->m.z;
  148.     return(t);
  149. }
  150.  
  151.  
  152. /*
  153.  * print a differential motion
  154.  */
  155.  
  156. printd(d, fp) /*::*/
  157. DIFF_PTR d;
  158. FILE *fp;
  159. {
  160.     fprintf(fp,
  161.     "tx %8.1e  ty %8.1e  tz %8.1e   rx %8.1e ry %8.1e rz %8.1e\n",
  162.     d->t.x, d->t.y, d->t.z, d->r.x, d->r.y, d->r.z);
  163. }
  164.  
  165.  
  166.  
  167. /*
  168.  * print a forces and moments
  169.  */
  170.  
  171. printm(d, fp) /*::*/
  172. FORCE_PTR d;
  173. FILE *fp;
  174. {
  175.     fprintf(fp,
  176.     "fx %8.1e  fy %8.1e  fz %8.1e   mx %8.1e my %8.1e mz %8.1e\n",
  177.     d->f.x, d->f.y, d->f.z, d->m.x , d->m.y, d->m.z);
  178. }
  179.