home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl028
< prev
next >
Wrap
Text File
|
1987-03-02
|
4KB
|
179 lines
/*
* RCCL Version 1.0 Author : Vincent Hayward
* School of Electrical Engineering
* Purdue University
* Dir : src
* File : diff.c
* Remarks : General purpose functions to deal with diff motions
* and forces.
* Usage : part of the library
*/
/*LINTLIBRARY*/
#include "../h/rccl.h"
/*
* make a TRSF out of a DIFF,
* n o a have to be united and squared (comply accumulates too much error)
*/
TRSF_PTR df_to_tr(t, d) /*::*/
register TRSF_PTR t;
register DIFF_PTR d;
{
t->p.x = d->t.x;
t->p.y = d->t.y;
t->p.z = d->t.z;
t->n.x = t->o.y = t->a.z = 1.;
t->a.y = -(t->o.z = d->r.x);
t->n.z = -(t->a.x = d->r.y);
t->o.x = -(t->n.y = d->r.z);
Unit(&t->a, &t->a);
Cross(&t->n, &t->o, &t->a);
Cross(&t->o, &t->a, &t->n);
Unit(&t->o, &t->o);
Cross(&t->n, &t->o, &t->a);
return(t);
}
/*
* make a DIFF out of a TRSF
*/
DIFF_PTR tr_to_df(d, t) /*::*/
register DIFF_PTR d;
register TRSF_PTR t;
{
d->t.x = t->p.x;
d->t.y = t->p.y;
d->t.z = t->p.z;
d->r.x = t->o.z;
d->r.y = t->a.x;
d->r.z = t->n.y;
return(d);
}
/*
* force frame transformation
*/
FORCE_PTR forcetr(fc, ff, tr) /*::*/
register FORCE_PTR fc, ff;
register TRSF_PTR tr;
{
VECT k;
/* k = f X p + m */
k.x = ff->f.y * tr->p.z - ff->f.z * tr->p.y + ff->m.x;
k.y = ff->f.z * tr->p.x - ff->f.x * tr->p.z + ff->m.y;
k.z = ff->f.x * tr->p.y - ff->f.y * tr->p.x + ff->m.z;
fc->m.x = tr->n.x * k.x + tr->n.y * k.y + tr->n.z * k.z;
fc->m.y = tr->o.x * k.x + tr->o.y * k.y + tr->o.z * k.z;
fc->m.z = tr->a.x * k.x + tr->a.y * k.y + tr->a.z * k.z;
fc->f.x = tr->n.x * ff->f.x + tr->n.y * ff->f.y + tr->n.z * ff->f.z;
fc->f.y = tr->o.x * ff->f.x + tr->o.y * ff->f.y + tr->o.z * ff->f.z;
fc->f.z = tr->a.x * ff->f.x + tr->a.y * ff->f.y + tr->a.z * ff->f.z;
return(fc);
}
/*
* differential transformation
*/
DIFF_PTR difftr(dc, dd, tr) /*::*/
register DIFF_PTR dc, dd;
register TRSF_PTR tr;
{
VECT k;
/* k = r X p + d */
k.x = dd->r.y * tr->p.z - dd->r.z * tr->p.y + dd->t.x;
k.y = dd->r.z * tr->p.x - dd->r.x * tr->p.z + dd->t.y;
k.z = dd->r.x * tr->p.y - dd->r.y * tr->p.x + dd->t.z;
dc->t.x = tr->n.x * k.x + tr->n.y * k.y + tr->n.z * k.z;
dc->t.y = tr->o.x * k.x + tr->o.y * k.y + tr->o.z * k.z;
dc->t.z = tr->a.x * k.x + tr->a.y * k.y + tr->a.z * k.z;
dc->r.x = tr->n.x * dd->r.x + tr->n.y * dd->r.y + tr->n.z * dd->r.z;
dc->r.y = tr->o.x * dd->r.x + tr->o.y * dd->r.y + tr->o.z * dd->r.z;
dc->r.z = tr->a.x * dd->r.x + tr->a.y * dd->r.y + tr->a.z * dd->r.z;
return(dc);
}
/*
* copy a DIFF struct
*/
DIFF_PTR assigndiff(t, o) /*::*/
register DIFF_PTR t, o;
{
t->t.x = o->t.x;
t->t.y = o->t.y;
t->t.z = o->t.z;
t->r.x = o->r.x;
t->r.y = o->r.y;
t->r.z = o->r.z;
return(t);
}
/*
* copy a FORCE struct
*/
FORCE_PTR assignforce(t, o) /*::*/
register FORCE_PTR t, o;
{
t->f.x = o->f.x;
t->f.y = o->f.y;
t->f.z = o->f.z;
t->m.x = o->m.x;
t->m.y = o->m.y;
t->m.z = o->m.z;
return(t);
}
/*
* print a differential motion
*/
printd(d, fp) /*::*/
DIFF_PTR d;
FILE *fp;
{
fprintf(fp,
"tx %8.1e ty %8.1e tz %8.1e rx %8.1e ry %8.1e rz %8.1e\n",
d->t.x, d->t.y, d->t.z, d->r.x, d->r.y, d->r.z);
}
/*
* print a forces and moments
*/
printm(d, fp) /*::*/
FORCE_PTR d;
FILE *fp;
{
fprintf(fp,
"fx %8.1e fy %8.1e fz %8.1e mx %8.1e my %8.1e mz %8.1e\n",
d->f.x, d->f.y, d->f.z, d->m.x , d->m.y, d->m.z);
}