p3dcoords.c

来自「speech signal process tools」· C语言 代码 · 共 1,240 行 · 第 1/2 页

/*
 * This material contains unpublished, proprietary software of 
 * Entropic Research Laboratory, Inc. Any reproduction, distribution, 
 * or publication of this work must be authorized in writing by Entropic 
 * Research Laboratory, Inc., and must bear the notice: 
 *
 *    "Copyright (c) 1986-1990  Entropic Speech, Inc. 
 *    "Copyright (c) 1990-1993  Entropic Research Laboratory, Inc. 
 *                   All rights reserved"
 *
 * The copyright notice above does not evidence any actual or intended 
 * publication of this source code.     
 *
 * Written by:  Rodney W. Johnson, Entropic Speech, Inc.
 * Checked by:
 * Revised by:
 *
 * Brief description:  Handle coordinate transformations.
 *
 */

static char *sccs_id = "@(#)p3dcoords.c	1.13	6/24/93	ESI/ERL";
 
#include <stdio.h>
#include <math.h>
#include <esps/esps.h>
#include <esps/unix.h>
#include <esps/constants.h>
#include "plot3d.h"


extern int  debug_level;
extern int  force_monochrome_plot;

extern void drawline();
extern void drawlines();
extern void drawpoint();
extern void drawing_style();

void	    update_mats();
void	    make_rmat(), update_rmat();
void	    make_imats();
int	    base_trans();

int	    can_width = 250,
	    can_height = 250;

int	    length = 150,
	    width = 250,
	    height = 400;
int	    box_dims[3] = {150, 250, 400};

double	    hskew = 0.0,
	    vskew = 0.0,
	    finv = 0.0005;		/* 1/finv = coord normal to screen
					   of center of projection.
					   (0 => parallel proj.) */
int	    ori = -1;			/* orientation (handedness) of axes.
					   -1 = left, 1 = right. */

double	    rot = 0.0,	    sin_psi = 0.0,	cos_psi = 1.0,
	    bear = 0.0,	    sin_theta = 0.0,	cos_theta = 1.0,
	    elev = 0.0,	    sin_phi = 0.0,	cos_phi = 1.0;
double	    xlow = 0.0,	    xhigh = 100.0,	xscale = 1.0,
	    ylow = 0.0,	    yhigh = 100.0,	yscale = 1.0,
	    zlow = 0.0,	    zhigh = 100.0,	zscale = 1.0;

		/* transformation matrices */

double	    tmat[3][4];		/* plot coordinates to canvas coordinates */
double	    smat[3][4];		/* data coordinates to canvas coordinates */
double	    rmat[3][3];		/* rotation matrix, used in updating rota-
				/* tion angles with mouse movements */
double	    amat[3][3];		/* axis-label coords to canvas coords */
double	    base_imat[3][3];	/* canvas to data coords in base plane */

double	    pmatx[3][4],	/* project on vertical planes in data */
	    pmaty[3][4];	/*     space, for checking visibility */
double	    pconstx, pconsty;	/* used with pmats; coordinates locating the
				   vertical planes */

				/* normal vectors to box faces */
int	    face_nor[6][3] =   {{-1,  0,  0}, { 1,  0,  0},
				{ 0, -1,  0}, { 0,  1,  0},
				{ 0,  0, -1}, { 0,  0,  1}};
				/* edge lists of box faces */
int	    face_edge[6][4] =  {{ 4,  8,  5,  9},
				{ 6, 11,  7, 10},
				{ 0, 10,  1,  8},
				{ 2,  9,  3, 11},
				{ 0,  4,  2,  6},
				{ 1,  7,  3,  5}};
				/* face lists of box edges */
int	    edge_face[12][2] = {{2, 4}, {2, 5}, {3, 4}, {3, 5},
				{0, 4}, {0, 5}, {1, 4}, {1, 5},
				{0, 2}, {0, 3}, {1, 2}, {1, 3}};
				/* vertex lists of box edges */
				/* directed low end to high end */
int	    edge_vert[12][2] = {{0, 4}, {1, 5}, {2, 6}, {3, 7},
				{0, 2}, {1, 3}, {4, 6}, {5, 7},
				{0, 1}, {2, 3}, {4, 5}, {6, 7}};
				/* edge parallel to x-axis (0), y-axis (1),
				   or z-axis (2)? */
int	    edge_dir[12] =     {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2};
				/* vertex lists of box faces
				   (counter-clockwise order as seen
				   from outside) */
int	    face_vert[6][4] =  {{0, 1, 3, 2},
				{4, 6, 7, 5},
				{0, 4, 5, 1},
				{2, 3, 7, 6},
				{0, 2, 6, 4},
				{1, 5, 7, 3}};
				/* normalized coordinates of box vertices
				   (mult by {length, width, height}
				   to get coords in plot space) */
int	    vert_coor[8][3] =  {{ 0,  0,  0},
				{ 0,  0,  1},
				{ 0,  1,  0},
				{ 0,  1,  1},
				{ 1,  0,  0},
				{ 1,  0,  1},
				{ 1,  1,  0},
				{ 1,  1,  1}};

/* numbering of box's
 *
 *                faces                               edges
 *
 *        (0)
 *          \      (5)
 *         _________|____                      _____(5)______
 *        |\  \     |    \                    |\             \
 *        | \       |   | \                   | \           | \
 *        |  \  \          \                  | (1)           (3)
 *        |   \         |   \                (8)  \        (9)  \
 *        |    \ ____________\                |    \ _____(7)____\
 *        |     |       |     |               |     |       |     |
 *   (2)-----   |           - |--(3)          |     |             |
 *        |_  _ |_  _  _|     |               |_  _ |(4)_  _|     |
 *         \    |        \    |                \  (10)          (11)
 *          \   |             |                 \   |         \   |
 *           \  |       \  \  |                 (0) |         (2) |
 *            \ |   |    \    |                   \ |           \ |
 *             \|_________\__\|                    \|_____(6)_____|
 *                  |      \
 *                 (4)     (1)
 *
 *
 *
 *               vertices
 *
 *
 *       (1)___________(3)
 *        |\             \
 *        | \           | \
 *        |  \             \
 *        |   \         |   \
 *        |    (5)___________(7)              z
 *        |     |       |     |               |
 *        |     |             |               |
 *       (0)_  _| _  _ (2)    |               |_____y
 *         \    |        \    |                \
 *          \   |             |                 \
 *           \  |          \  |                  x
 *            \ |             |
 *             (4)___________(6)
 *
 */



void
set_canv_dimens(w, h)
    int	    w, h;
{
    can_width = w;
    can_height = h;
    update_mats();
}


void
set_box_len(n)
    int     n;
{
    length = box_dims[0] = n;
    xscale = length / (xhigh - xlow);
    update_mats();
}

int
get_box_len()
{
    return length;
}


void
set_box_wid(n)
    int     n;
{
    width = box_dims[1] = n;
    yscale = width / (yhigh - ylow);
    update_mats();
}

int
get_box_wid()
{
    return width;
}


void
set_box_hgt(n)
    int     n;
{
    height = box_dims[2] = n;
    zscale = height / (zhigh - zlow);
    update_mats();
}

int
get_box_hgt()
{
    return height;
}


void
set_hskew(x)
    double  x;
{
    hskew = x;
    update_mats();
}

double
get_hskew()
{
    return hskew;
}
   

void
set_vskew(x)
    double  x;
{
    vskew = x;
    update_mats();
}

double
get_vskew()
{
    return vskew;
}
   

void
set_finv(x)
    double x;
{
    finv = 0.00001*x;
    update_mats();
}

double
get_finv()
{
    return 100000.0*finv;
}


void
set_ori(n)
    int	    n;
{
    if (n == ORI_RIGHT)
	ori = 1;
    else
	ori = -1;

    update_mats();
}

int
get_ori()
{
    return (ori == 1) ? ORI_RIGHT : ORI_LEFT;
}

void
set_rot(x)
    double x;
{
    rot = x;
    sin_psi = sin(rot);
    cos_psi = cos(rot);
    update_mats();
}

double
get_rot()
{
    return rot;
}


void
set_bear(x)
    double x;
{
    bear = x;
    sin_theta = sin(bear);
    cos_theta = cos(bear);
    update_mats();
}

double
get_bear()
{
    return bear;
}


void
set_elev(x)
    double x;
{
    elev = x;
    sin_phi = sin(elev);
    cos_phi = cos(elev);
    update_mats();
}

double
get_elev()
{
    return elev;
}


void
update_mats()
{
    double  m[4][4];
    int	    i, j;
    double  ctr_x, ctr_y, ctr_z, ctr_u, ctr_v;

    /* matrix for rotation in xy-, xz-, and yz-planes (homogeneous coords) */
    /*
     *       /          0 \
     *   m = |   rmat   0 |
     *       |          0 |
     *       \ 0  0  0  1 /
     */

    make_rmat();
    for (i = 0; i < 3; i++)
    {
	for (j = 0; j < 3; j++)
	    m[i][j] = rmat[i][j];
	m[i][3] = 0.0;
    }
    for (j = 0; j < 3; j++)
	m[3][j] = 0.0;
    m[3][3] = 1.0;

    /* precede with orientation setting */
    /*
     *         / -1    0    0  0 \
     *   m = m |  0  - ori  0  0 |
     *         |  0    0    1  0 |
     *         \  0    0    0  1 /
     */

    for (i = 0; i < 3; i++)
    {
	m[i][0] = -m[i][0];
	m[i][1] = -ori*m[i][1];
    }

    /* ... and with translation of center of box to origin */
    /*
     *         / 1  0  0  - length/2 \
     *   m = m | 0  1  0  - width/2  |
     *         | 0  0  1  - height/2 |
     *         \ 0  0  0       1     /
     */

    ctr_x = 0.5*length;	ctr_y = 0.5*width;  ctr_z = 0.5*height;
    for (i = 0; i < 3; i++)
	m[i][3] = - m[i][0]*ctr_x - m[i][1]*ctr_y - m[i][2]*ctr_z;

    /* follow with projection into yz-plane */
    /*
     *       / - hskew  1  0  0 \
     *   m = | - vskew  0  1  0 | m
     *       \ - finv   0  0  1 /
     */

    for (j = 0; j < 4; j++)
    {
	m[1][j] -= hskew*m[0][j];
	m[2][j] -= vskew*m[0][j];
	m[3][j] -= finv*m[0][j];
    }

    /* map into canvas coordinates (translate origin to center of canvas) */
    /*
     *          / 1    0  can_width/2  \
     *   tmat = | 0  - 1  can_height/2 | m
     *          \ 0    0        1      /
     */

    ctr_u = 0.5*can_width;	ctr_v = 0.5*can_height;
    for (j = 0; j < 4; j++)
    {
	tmat[0][j] = m[1][j] + ctr_u*m[3][j];
	tmat[1][j] = - m[2][j] + ctr_v*m[3][j];
	tmat[2][j] = m[3][j];
    }

    if (debug_level >= 2)
	printmat("tmat", &tmat[0][0], 3, 4);

    /* tmat defined--now do smat */
    /* precede transformation with scaling from data units to plot units */
    /*
     *               / xscale     0       0    0 \
     *   smat = tmat |    0    yscale     0    0 |
     *               |    0       0    zscale  0 |
     *               \    0       0       0    1 /
     */

    for (i = 0; i < 3; i++)
    {
	smat[i][0] = tmat[i][0]*xscale;
	smat[i][1] = tmat[i][1]*yscale;
	smat[i][2] = tmat[i][2]*zscale;
	smat[i][3] = tmat[i][3];
    }
	
    if (debug_level >= 2)
	printmat("scale", &smat[0][0], 3, 4);

    /* precede that with translation of coordinate low limits to origin */
    /*
     *               / 1  0  0  - xlow \
     *   smat = smat | 0  1  0  - ylow |
     *               | 0  0  1  - zlow |
     *               \ 0  0  0     1   /
     */

    for (i = 0; i < 3; i++)
	smat[i][3] -= (smat[i][0]*xlow + smat[i][1]*ylow + smat[i][2]*zlow);

    if (debug_level >= 2)
	printmat("trans smat", &smat[0][0], 3, 4);

    make_imats();
}


void
make_rmat()
{
    double  t;
    int	    i, j;

    for (i = 0; i < 3; i++)
	for (j = 0; j < 3; j++)
	    rmat[i][j] = 0.0;

    /* rotation in xy-plane */
    /*
     *          /   cos theta  sin theta  0 \
     *   rmat = | - sin theta  cos theta  0 |
     *          \      0           0      1 /
     */

    rmat[0][0] = cos_theta;	rmat[0][1] = sin_theta;
    rmat[1][0] = -sin_theta;	rmat[1][1] = cos_theta;
    rmat[2][2] = 1.0;

    /* rotation in xz-plane */
    /*
     *          /   cos phi  0  sin phi \
     *   rmat = |     0      1     0    | rmat
     *          \ - sin phi  0  cos phi /
     */

    for (j = 0; j < 3; j++)
    {
	t = cos_phi*rmat[0][j] + sin_phi*rmat[2][j];
	rmat[2][j] = - sin_phi*rmat[0][j] + cos_phi*rmat[2][j];
	rmat[0][j] = t;
    }

    /* rotation in yz-plane */
    /*
     *          / 1      0         0    \
     *   rmat = | 0    cos psi  sin psi | rmat
     *          \ 0  - sin psi  cos psi /
     */

    for (j = 0; j < 3; j++)
    {
	t = cos_psi*rmat[1][j] + sin_psi*rmat[2][j];
	rmat[2][j] = - sin_psi*rmat[1][j] + cos_psi*rmat[2][j];
	rmat[1][j] = t;
    }

    if (debug_level >= 2)
    {
	(void) fprintf(stderr, "make_rmat:\n  sin theta %g,\tcos theta %g\n",
			sin_theta, cos_theta);
	(void) fprintf(stderr, "  sin phi   %g,\tcos phi   %g\n",
			sin_phi, cos_phi);
	(void) fprintf(stderr, "  sin psi   %g,\tcos psi   %g\n",
			sin_psi, cos_psi);
	printmat("rmat", &rmat[0][0], 3, 3);
    }
	
}


void
update_rmat(th, ph, theta, phi, psi)
    double  th, ph, *theta, *phi, *psi;
{
    if (th == 0.0 && ph == 0.0)
    {
	*theta = bear;	*phi = elev;	*psi = rot;
    }
    else
    {
	double	r, c, s, t, ps, dif;
	int	j;

	r = sqrt(th*th + ph*ph);
	th /= r;	ph /= r;
	c = cos(r);	s = sin(r);

	/* transform axis of rotation to z axis */
	/*
	 *          / 1   0    0 \
	 *   rmat = | 0   th  ph | rmat
	 *          \ 0  -ph  th /
	 */
	
	for (j = 0; j < 3; j++)
	{
	    t = th*rmat[1][j] + ph*rmat[2][j];
	    rmat[2][j] = -ph*rmat[1][j] + th*rmat[2][j];
	    rmat[1][j] = t;
	}

	/* do the rotation */
	/*
	 *          /  c  s  0 \
	 *   rmat = | -s  c  0 | rmat
	 *          \  0  0  1 /
	 */
	
	for (j = 0; j < 3; j++)
	{
	    t = c*rmat[0][j] + s*rmat[1][j];
	    rmat[1][j] = -s*rmat[0][j] + c*rmat[1][j];
	    rmat[0][j] = t;
	}

	/* transform z axis back to rotation axis */
	/*
	 *          / 1   0   0  \
	 *   rmat = | 0  th  -ph | rmat
	 *          \ 0  ph   th /
	 */

	for (j = 0; j < 3; j++)
	{
	    t = th*rmat[1][j] - ph*rmat[2][j];
	    rmat[2][j] = ph*rmat[1][j] + th*rmat[2][j];
	    rmat[1][j] = t;
	}

	if (debug_level >= 2)
	{
	    (void) fprintf(stderr, "update_rmat:  th %g, ph %g\n", th, ph);
	    printmat("rmat", &rmat[0][0], 3, 3);
	}

	/* solve for new Euler angles */

	*phi = asin(rmat[0][2]);

	s = rmat[1][2];		/* sin psi cos phi */
	c = rmat[2][2];		/* cos psi cos phi */
	ps = (s == 0.0 && c == 0.0) ? 0.0 : atan2(s, c);
				/* approx. psi */

	s = rmat[0][1];	/* cos phi sin theta */
	c = rmat[0][0];	/* cos phi cos theta */
	th = (s == 0.0 && c == 0.0) ? 0.0 : atan2(s, c);
				/* approx. theta */

	/* psi and theta may be poorly determined if cos phi is small;
	 * but their sum or difference is well determined.
	 */
/*!*//* if cos phi not small output ps and th ... */

	if (*phi > 0.0)
	{
	    s = - rmat[1][0] - rmat[2][1];
				/* (1 + sin phi) sin (psi + theta) */
	    c = rmat[1][1] - rmat[2][0];
				/* (1 + sin phi) cos (psi + theta) */
	    dif = atan2(s, c) - (ps + th);