home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Between Heaven & Hell 2
/
BetweenHeavenHell.cdr
/
500
/
470
/
rccl034
< prev
next >
Wrap
Text File
|
1987-03-02
|
11KB
|
557 lines
/*
* RCCL Version 1.0 Author : Vincent Hayward
* School of Electrical Engineering
* Purdue University
* Dir : src
* File : trans.c
* Remarks : All the vector and transform utility functions.
* Usage : part of the library
*/
/*LINTLIBRARY*/
#include "../h/rccl.h"
#include "../h/umac.h"
/*
* make a copy of a vector
*/
VECT_PTR assignvect(r, v) /*::*/
register VECT_PTR r, v;
{
r->x = v->x;
r->y = v->y;
r->z = v->z;
return(r);
}
/*
* computes dot product of two vectors
*/
real dot(u, v) /*::*/
register VECT_PTR u, v;
{
return(u->x * v->x + u->y * v->y + u->z * v->z);
}
/*
* computes the cross product of two vectors
*/
VECT_PTR cross(r, u, v) /*::*/
register VECT_PTR r, u, v;
{
r->x = u->y * v->z - u->z * v->y;
r->y = u->z * v->x - u->x * v->z;
r->z = u->x * v->y - u->y * v->x;
return(r);
}
/*
* brings the magnitude of a vector to unity
*/
VECT_PTR unit(v, u) /*::*/
register VECT_PTR v, u;
{
real m;
m = sqrt(dot(u, u));
if (m < SMALL) {
giveup("cannot unit vector", YES);
}
v->x = u->x / m;
v->y = u->y / m;
v->z = u->z / m;
return(v);
}
/*
* assigns t (rhs) transform to r (lhs) transform
*/
TRSF_PTR assigntr(r, t) /*::*/
register TRSF_PTR r, t;
{
r->n.x = t->n.x;
r->o.x = t->o.x;
r->a.x = t->a.x;
r->p.x = t->p.x;
r->n.y = t->n.y;
r->o.y = t->o.y;
r->a.y = t->a.y;
r->p.y = t->p.y;
r->n.z = t->n.z;
r->o.z = t->o.z;
r->a.z = t->a.z;
r->p.z = t->p.z;
return(r);
}
/*
* assigns translation part of t (rhs) to r (lhs)
*/
TRSF_PTR taketrsl(r, t) /*::*/
register TRSF_PTR r, t;
{
r->p.x = t->p.x;
r->p.y = t->p.y;
r->p.z = t->p.z;
return(r);
}
/*
* assigns rotational part of t (rhs) to r (lhs)
*/
TRSF_PTR takerot(r, t) /*::*/
register TRSF_PTR r, t;
{
r->n.x = t->n.x;
r->o.x = t->o.x;
r->a.x = t->a.x;
r->n.y = t->n.y;
r->o.y = t->o.y;
r->a.y = t->a.y;
r->n.z = t->n.z;
r->o.z = t->o.z;
r->a.z = t->a.z;
return(r);
}
/*
* returns in r matrix the product of t by u
* r t u should be different matrices
*/
TRSF_PTR trmult(r, t, u) /*::*/
register TRSF_PTR r, t, u;
{
r->n.x = t->n.x * u->n.x + t->o.x * u->n.y + t->a.x * u->n.z;
r->n.y = t->n.y * u->n.x + t->o.y * u->n.y + t->a.y * u->n.z;
r->n.z = t->n.z * u->n.x + t->o.z * u->n.y + t->a.z * u->n.z;
r->o.x = t->n.x * u->o.x + t->o.x * u->o.y + t->a.x * u->o.z;
r->o.y = t->n.y * u->o.x + t->o.y * u->o.y + t->a.y * u->o.z;
r->o.z = t->n.z * u->o.x + t->o.z * u->o.y + t->a.z * u->o.z;
r->a.x = t->n.x * u->a.x + t->o.x * u->a.y + t->a.x * u->a.z;
r->a.y = t->n.y * u->a.x + t->o.y * u->a.y + t->a.y * u->a.z;
r->a.z = t->n.z * u->a.x + t->o.z * u->a.y + t->a.z * u->a.z;
r->p.x = t->n.x * u->p.x + t->o.x * u->p.y + t->a.x * u->p.z + t->p.x;
r->p.y = t->n.y * u->p.x + t->o.y * u->p.y + t->a.y * u->p.z + t->p.y;
r->p.z = t->n.z * u->p.x + t->o.z * u->p.y + t->a.z * u->p.z + t->p.z;
return(r);
}
/*
* matrix mult in place
*/
TRSF_PTR trmultinp(r, m) /*::*/
register TRSF_PTR r, m;
{
TRSF temp;
return(trmult(r, assigntr(&temp, r), m));
}
/*
* matrix mult by inverse
*/
TRSF_PTR trmultinv(r, m) /*::*/
register TRSF_PTR r, m;
{
TRSF temp1, temp2;
return(trmult(r, assigntr(&temp2, r), invert(&temp1, m)));
}
/*
* computes the inverse of a transform
*/
TRSF_PTR invert(i, t) /*::*/
register TRSF_PTR i, t;
{
i->n.x = t->n.x;
i->n.y = t->o.x;
i->n.z = t->a.x;
i->o.x = t->n.y;
i->o.y = t->o.y;
i->o.z = t->a.y;
i->a.x = t->n.z;
i->a.y = t->o.z;
i->a.z = t->a.z;
i->p.x = - (t->p.x * t->n.x + t->p.y * t->n.y + t->p.z * t->n.z);
i->p.y = - (t->p.x * t->o.x + t->p.y * t->o.y + t->p.z * t->o.z);
i->p.z = - (t->p.x * t->a.x + t->p.y * t->a.y + t->p.z * t->a.z);
return(i);
}
/*
* invert in place
*/
TRSF_PTR invertinp(t) /*::*/
TRSF_PTR t;
{
TRSF temp;
return(invert(t, assigntr(&temp, t)));
}
/*
* set a pure translation transform
*/
TRSF_PTR trsl(t, x, y, z) /*::*/
register TRSF_PTR t;
real x, y, z;
{
t->p.x = x;
t->p.y = y;
t->p.z = z;
return(t);
}
/*
* multiply a transform by a pure translation transform
*/
TRSF_PTR trslm(t, x, y, z) /*::*/
register TRSF_PTR t;
real x, y, z;
{
static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
temp.n.x = temp.o.y = temp.a.z = 1.;
temp.n.y = temp.n.z = temp.o.x = temp.o.z = temp.a.x = temp.a.y = 0.;
return(trmultinp(t, trsl(&temp, x, y, z)));
}
/*
* set a transform given the a o vectors
*/
TRSF_PTR vao(t, ax, ay, az, ox, oy, oz) /*::*/
register TRSF_PTR t;
real ax, ay, az, ox, oy, oz;
{
t->a.x = ax;
t->a.y = ay;
t->a.z = az;
t->o.x = ox;
t->o.y = oy;
t->o.z = oz;
(void) unit(&t->a, &t->a);
(void) cross(&t->n, &t->o, &t->a);
(void) cross(&t->o, &t->a, &t->n);
(void) unit(&t->o, &t->o);
(void) cross(&t->n, &t->o, &t->a);
return(t);
}
/*
* multiply a transform by a rotation expressed with a o vectors
*/
TRSF_PTR vaom(t, ax, ay, az, ox, oy, oz) /*::*/
register TRSF_PTR t;
real ax, ay, az, ox, oy, oz;
{
static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
temp.p.z = temp.p.y = temp.p.z = 0.;
return(trmultinp(t, vao(&temp, ax, ay, az, ox, oy, oz)));
}
/*
* sets the terms of a transform given a rotation theta around a vector k
*/
TRSF_PTR rot(t, k, h) /*::*/
register TRSF_PTR t;
register VECT_PTR k;
real h;
{
VECT kl;
real s, c, ver, x, y, z, xv, yv, zv, xs, ys, zs;
(void) unit(&kl, k);
s = sin(dgtord_m * h);
ver = 1. - (c = cos(dgtord_m * h));
xv = ver * (x = kl.x);
xs = x * s;
yv = ver * (y = kl.y);
ys = y * s;
zv = ver * (z = kl.z);
zs = z * s;
t->n.x = x * xv + c;
t->n.y = x * yv + zs;
t->n.z = x * zv - ys;
t->o.x = y * xv - zs;
t->o.y = y * yv + c;
t->o.z = y * zv + xs;
t->a.x = z * xv + ys;
t->a.y = z * yv - xs;
t->a.z = z * zv + c;
return(t);
}
/*
* multiply a transform by a rotation about a vector
*/
TRSF_PTR rotm(t, k, h) /*::*/
register TRSF_PTR t;
register VECT_PTR k;
real h;
{
static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
temp.p.z = temp.p.y = temp.p.z = 0.;
return(trmultinp(t, rot(&temp, k, h)));
}
/*
* sets the terms of a transform given the euler angles
* expressed in degrees
*/
TRSF_PTR eul(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
real sphi, sthe, spsi, cphi, cthe, cpsi;
sphi = sin(dgtord_m * phi);
cphi = cos(dgtord_m * phi);
sthe = sin(dgtord_m * the);
cthe = cos(dgtord_m * the);
spsi = sin(dgtord_m * psi);
cpsi = cos(dgtord_m * psi);
t->n.x = cphi * cthe * cpsi - sphi * spsi;
t->n.y = sphi * cthe * cpsi + cphi * spsi;
t->n.z = - sthe * cpsi;
t->o.x = - cphi * cthe * spsi - sphi * cpsi;
t->o.y = - sphi * cthe * spsi + cphi * cpsi;
t->o.z = sthe * spsi;
t->a.x = cphi * sthe;
t->a.y = sphi * sthe;
t->a.z = cthe;
return(t);
}
/*
* multiply a transform by a rotation expressed with euler angles
*/
TRSF_PTR eulm(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
temp.p.z = temp.p.y = temp.p.z = 0.;
return(t, trmultinp(t, eul(&temp, phi, the, psi)));
}
/*
* returns the euler angles of the rotation part of a transform with
* euler angles given in degrees
*/
noatoeul(phi, the, psi, t) /*::*/
real *phi, *the, *psi; /* pointers to angles */
register TRSF_PTR t;
{
real sphi, cphi, p;
if (FABS(t->a.y) > SMALL || FABS(t->a.x) > SMALL) {
*phi = rdtodg_m * (p = atan2(t->a.y, t->a.x));
sphi = sin(p);
cphi = cos(p);
*the = rdtodg_m * atan2(cphi * t->a.x + sphi * t->a.y, t->a.z);
*psi = rdtodg_m * atan2(- sphi * t->n.x + cphi * t->n.y ,
- sphi * t->o.x + cphi * t->o.y);
}
else {
*phi = 0.;
*the = rdtodg_m * atan2(t->a.x, t->a.z);
*psi = rdtodg_m * atan2(t->n.y, t->o.y);
}
}
/*
* sets the term of a transform with a rotation expressed with rpy
* angles in degrees
*/
TRSF_PTR rpy(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
real sphi, sthe, spsi, cphi, cthe, cpsi;
sphi = sin(dgtord_m * phi);
cphi = cos(dgtord_m * phi);
sthe = sin(dgtord_m * the);
cthe = cos(dgtord_m * the);
spsi = sin(dgtord_m * psi);
cpsi = cos(dgtord_m * psi);
t->n.x = cphi * cthe;
t->n.y = sphi * cthe;
t->n.z = - sthe;
t->o.x = cphi * sthe * spsi - sphi * cpsi;
t->o.y = sphi * sthe * spsi + cphi * cpsi;
t->o.z = cthe * spsi;
t->a.x = cphi * sthe * cpsi + sphi * spsi;
t->a.y = sphi * sthe * cpsi - cphi * spsi;
t->a.z = cthe * cpsi;
return(t);
}
/*
* multilpy a transform with a rotation expressed with rpy anlges
*/
TRSF_PTR rpym(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};
temp.p.z = temp.p.y = temp.p.z = 0.;
return(t, trmultinp(t, rpy(&temp, phi, the, psi)));
}
/*
* computes rpy angles from the n o a vectors of a transform
*/
noatorpy(phi, the, psi, t) /*::*/
real *phi, *the, *psi;
register TRSF_PTR t;
{
real sphi, cphi, nx, ny, p;
nx = t->n.x;
ny = t->n.y;
if (FABS(nx) < SMALL && FABS(ny) < SMALL) {
*phi = 0.;
*the = rdtodg_m * atan2(- t->n.z, nx);
*psi = rdtodg_m * atan2(- t->a.y, t->o.y);
}
else {
*phi = rdtodg_m * (p = atan2(ny, nx));
sphi = sin(p);
cphi = cos(p);
*the = rdtodg_m * atan2(- t->n.z, cphi * nx + sphi * ny);
*psi = rdtodg_m * atan2(sphi * t->a.x - cphi * t->a.y ,
cphi * t->o.y - sphi * t->o.x);
}
}
/*
* prints out the numerical information
*/
printr(t, fp) /*::*/
TRSF_PTR t;
FILE *fp;
{
fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
t->n.x, t->o.x, t->a.x, t->p.x);
fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
t->n.y, t->o.y, t->a.y, t->p.y);
fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
t->n.z, t->o.z, t->a.z, t->p.z);
}
/*
* prints name value and rpy and euler angles of a transform
*/
printrn(t, fp) /*::*/
TRSF_PTR t;
FILE *fp;
{
fprintf(fp, "%s :\n", t->name);
printr(t, fp);
printe(t, fp);
printy(t, fp);
}
/*
* prints out the euler representation of a transform
*/
printe(e, fp) /*::*/
TRSF_PTR e;
FILE *fp;
{
real p, t, s;
noatoeul(&p, &t, &s, e);
fprintf(fp,
"EUL x:%-7.3f y:%-7.3f z:%-7.3f phi:%-7.3f the:%-7.3f psi:%-7.3f\n",
e->p.x, e->p.y, e->p.z, p, t, s);
}
/*
* prints out the rpy representation of a transform
*/
printy(e, fp) /*::*/
TRSF_PTR e;
FILE *fp;
{
real p, t, s;
noatorpy(&p, &t, &s, e);
fprintf(fp,
"RPY x:%-7.3f y:%-7.3f z:%-7.3f phi:%-7.3f the:%-7.3f psi:%-7.3f\n",
e->p.x, e->p.y, e->p.z, p, t, s);
}