geometry.c

#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <string.h>

#ifdef sgi
#include <sys/endian.h>
#endif

#ifdef WIN32
# include "winjunk.h"
#endif

#include "geometry.h"

Matrix Tidentity = { 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1 };

void vrotxy( register Point *dst, CONST Point *src, CONST float cs[2] )
{
   dst->x[0] = src->x[0]*cs[0] - src->x[1]*cs[1];
   dst->x[1] = src->x[0]*cs[1] + src->x[1]*cs[0];
   dst->x[2] = src->x[2];
}

#define	VDOT(a, b) \
	((a)->x[0]*(b)->x[0] + (a)->x[1]*(b)->x[1] + (a)->x[2]*(b)->x[2])

#define VROTXY(dst, src, cs) \
	(dst)->x[0] = (src)->x[0]*cs[0] - (src)->x[1]*cs[1], \
	(dst)->x[1] = (src)->x[0]*cs[1] + (src)->x[1]*cs[0], \
	(dst)->x[2] = (src)->x[2]

#define	VMID(dst, a, b) \
	(dst)->x[0] = .5*((a)->x[0] + (b)->x[0]), \
	(dst)->x[1] = .5*((a)->x[1] + (b)->x[1]), \
	(dst)->x[2] = .5*((a)->x[2] + (b)->x[2])

void vadd( register Point *dst, CONST Point *a, CONST Point *b )
{
   dst->x[0] = a->x[0] + b->x[0];
   dst->x[1] = a->x[1] + b->x[1];
   dst->x[2] = a->x[2] + b->x[2];
}
void vsub( register Point *dst, CONST Point *a, CONST Point *b )
{
   dst->x[0] = a->x[0] - b->x[0];
   dst->x[1] = a->x[1] - b->x[1];
   dst->x[2] = a->x[2] - b->x[2];
}

void vcross( register Point *dst, register CONST Point *a, register CONST Point *b )
{
   dst->x[0] = a->x[1]*b->x[2] - a->x[2]*b->x[1];
   dst->x[1] = a->x[2]*b->x[0] - a->x[0]*b->x[2];
   dst->x[2] = a->x[0]*b->x[1] - a->x[1]*b->x[0];
}


float vdot( CONST Point *a, CONST Point *b ) {
   return a->x[0]*b->x[0] + a->x[1]*b->x[1] + a->x[2]*b->x[2];
}

void vscale( register Point *dst, float s, register CONST Point *src )
{
   dst->x[0] = s*src->x[0];
   dst->x[1] = s*src->x[1];
   dst->x[2] = s*src->x[2];
}

void vsadd( Point *dst, CONST Point *a, float sb, CONST Point *b )
{
   dst->x[0] = a->x[0] + sb * b->x[0];
   dst->x[1] = a->x[1] + sb * b->x[1];
   dst->x[2] = a->x[2] + sb * b->x[2];
}

float vdist( CONST Point *p1, CONST Point *p2 ) {
   Point d;
   vsub(&d, p1, p2);
   return vlength(&d);
}

float vlength( CONST Point *v ) {
   return (float)sqrt(VDOT(v, v));
}

float vunit( register Point *dst, register CONST Point *src ) {
   float s = (float)sqrt(VDOT(src, src));
   float scl = s>0 ? 1.0f / s : 0;
   vscale( dst, scl, src );
   return s;
}

/* along = onto * (vec . onto / onto . onto)
 * perp  = vec - along
 */
void vproj( Point *along, Point *perp, CONST Point *vec, CONST Point *onto ) {
    float mag2 = VDOT(onto, onto);
    float dot = VDOT(vec, onto);
    float s = (mag2 > 0) ? dot / mag2 : 0;
    Point talong;
    if(along == NULL) along = &talong;
    vscale( along, s, onto );
    if(perp != NULL)
	vsadd( perp, vec, -1, along );
}

/*
 * vtfmvector() transforms a vector (in homog coords, [x,y,z,0]) by a matrix.
 * vtfmpoint() transforms a point  [x,y,z,1].
 * The difference is that vtfmpoint() includes the matrix's translation part
 * and vtfmvector() doesn't.
 */

void vuntfmvector( Point *dst, register CONST Point *src, register CONST Matrix *T )
{
  int i;
  for(i = 0; i < 3; i++)
    dst->x[i] = src->x[0]*T->m[4*i] + src->x[1]*T->m[4*i+1] + src->x[2]*T->m[4*i+2];
}
void vtfmvector( Point *dst, register CONST Point *src, register CONST Matrix *T )
{
  int i;
  for(i = 0; i < 3; i++)
    dst->x[i] = src->x[0]*T->m[i] + src->x[1]*T->m[i+4] + src->x[2]*T->m[i+8];
}
void vtfmpoint( Point *dst, register CONST Point *src, register CONST Matrix *T )
{
  int i;
  for(i = 0; i < 3; i++)
    dst->x[i] = src->x[0]*T->m[i] + src->x[1]*T->m[i+4] + src->x[2]*T->m[i+8] + T->m[i+12];
}

void vgettranslation( Point *dst, CONST Matrix *T )
{
  memcpy(dst->x, &T->m[4*3+0], 3*sizeof(float));
}

void vsettranslation( Matrix *T, CONST Point *src )
{
  memcpy(&T->m[4*3+0], src->x, 3*sizeof(float));
}

/* Invert a matrix, assuming it's a Euclidean isometry
 * plus possibly uniform scaling.
 */
void eucinv( Matrix *dst, CONST Matrix *src )
{
  int i, j;
  float s = VDOT((Point *)src, (Point *)src);
  Point trans;
  Matrix T;
  if(src == dst) {
    T = *src;
    src = &T;
  }
  for(i = 0; i < 3; i++) {
    for(j = 0; j < 3; j++)
	dst->m[i*4+j] = src->m[j*4+i] / s;
    dst->m[i*4+3] = 0;
  }
  vtfmvector( &trans, (Point *)&src->m[4*3+0], dst );
  vscale( (Point *)&dst->m[3*4+0], -1, &trans );
  dst->m[3*4+3] = 1;
}

void mcopy( Matrix *dst, CONST Matrix *src )
{
  memcpy( dst, src, sizeof(Matrix) );
}

void mmmul( Matrix *dst, CONST Matrix *a, CONST Matrix *b )
{
  int i, irow, j;
  Matrix tmp;
  if(dst == a || dst == b) {
    mmmul( &tmp, a, b );
    *dst = tmp;
    return;
  }
  for(i = 0; i < 4; i++) {
    irow = i*4;
    for(j = 0; j < 4; j++)
	dst->m[irow+j] = a->m[irow]*b->m[j] + a->m[irow+1]*b->m[1*4+j]
		       + a->m[irow+2]*b->m[2*4+j] + a->m[irow+3]*b->m[3*4+j];
  }
}

/* Construct matrix for geodesic rotation from "a" to "b".
 */
void grotation( Matrix *Trot, CONST Point *va, CONST Point *vb )
{
  Point a, b, aperp;
  float ab_1, apb;
  int i, j;

  mcopy( Trot, &Tidentity );
  if(vunit(&a, va) == 0 || vunit(&b, vb) == 0)
    return;
  ab_1 = VDOT(&a,&b) - 1;
  vproj( NULL, &aperp, &b, &a );
  if(vunit(&aperp, &aperp) == 0) {
    if(ab_1 >= -1)
	return;		/* Vectors are identical: no rotation */

    /* Otherwise, vectors are oppositely directed.
     * Rotate in an arbitrary plane which includes them.
     */
    aperp.x[ fabs(a.x[0]) < .7 ? 0 : 1 ] = 1;
    vproj( NULL, &aperp, &aperp, &a );
    vunit(&aperp, &aperp);
  }
  apb = VDOT(&aperp, &b);
  for(i = 0; i < 3; i++) {
    float acoef = a.x[i]*ab_1 - aperp.x[i]*apb;
    float apcoef = aperp.x[i]*ab_1 + a.x[i]*apb;
    for(j = 0; j < 3; j++)
	Trot->m[i*4+j] += a.x[j]*acoef + aperp.x[j]*apcoef;
  }
}

/*
 * Conjugate a transformation as needed for interactive positioning,
 * applying it in a given coordinate frame, and conjugating rotations
 * to fix a given point.
 * Coord abbreviations:  "o" (object) "w" (world) "f" (frame) "cen" center.
 * Tf2w and Tw2f are frame-to-world transform and its inverse.
 *  (Either may be NULL, in which case it's computed from the other.)
 *  (If both are NULL, we assume frame == world.)
 * Rotation-fixing point is given by either:
 *  pcenw -- the point in world coordinates, if non-NULL, or
 *  pcenf -- the point in frame coordinates, if non-NULL.
 * If both are NULL, the point is taken to be (0,0,0) in frame coordinates.
 */
void mconjugate( Matrix *To2wout, CONST Matrix *To2win, CONST Matrix *Tincrf,
		CONST Matrix *Tf2w, CONST Matrix *Tw2f,
		CONST Point *pcenw, CONST Point *pcenf )
{
  Matrix t_incrf, t_f2w, t_w2f;
  Matrix t1, t2;
  Point pt1, pt2, tp_cenf;

  if(Tf2w == NULL && Tw2f != NULL) {
    Tf2w = &t_f2w;
    eucinv((Matrix *)Tf2w, Tw2f);
  } else if(Tw2f == NULL && Tf2w != NULL) {
    Tw2f = &t_w2f;
    eucinv((Matrix *)Tw2f, Tf2w);
  }
  if(pcenf == NULL && pcenw != NULL) {
    if(Tw2f != NULL) {
	vtfmpoint(&tp_cenf, pcenw, Tw2f);
	pcenf = &tp_cenf;
    } else {
	pcenf = pcenw;
    }
  }
  if(pcenf != NULL) {
    t_incrf = *Tincrf;
    vtfmvector( &pt1, pcenf, &t_incrf );
    vsub( &pt1, pcenf, &pt1 );
    vgettranslation( &pt2, &t_incrf );
    vadd( &pt1, &pt1, &pt2 );
    vsettranslation( &t_incrf, &pt1 );
    Tincrf = &t_incrf;
  }

  if(Tf2w != NULL) {
    mmmul( &t1, Tw2f, Tincrf );
    mmmul( &t2, &t1,         Tf2w );
    mmmul( To2wout, To2win, &t2 );
  } else {
    mmmul( To2wout, To2win, Tincrf );
  }
}

void mrotation( Matrix *rot, float degrees, char xyz ) {
  float s = sin(degrees * (M_PI/180));
  float c = cos(degrees * (M_PI/180));
  int a = (xyz - 'x' + 1) % 3;
  int b = (xyz - 'x' + 2) % 3;

  *rot = Tidentity;
  rot->m[a*4+a] = c;
  rot->m[a*4+b] = s;
  rot->m[b*4+a] = -s;
  rot->m[b*4+b] = c;
}

void mscaling( Matrix *scale, float sx, float sy, float sz ) {
  *scale = Tidentity;
  scale->m[0*4+0] = sx;
  scale->m[1*4+1] = sy;
  scale->m[2*4+2] = sz;
}

void mtranslation( Matrix *tran, float tx, float ty, float tz ) {
  *tran = Tidentity;
  tran->m[3*4+0] = tx;
  tran->m[3*4+1] = ty;
  tran->m[3*4+2] = tz;
}


/*
 * Convert the rotation part of a Euclidean isometry+uniform-scaling matrix
 * into a unit quaternion, with non-negative real part.
 * Returns the real part of the quaternion, with the three imaginary components
 * left in iquat->x[0,1,2].
 */
float tfm2quat( Point *iquat, CONST Matrix *T )
{
  float mag, sinhalf, trace;
  float scl = vlength((Point *)T);	/* gauge scaling from 1st row */
  Point axis;

#define Tij(i,j) T->m[(i)*4+(j)]

  trace = scl==0 ? 3 : (Tij(0,0) + Tij(1,1) + Tij(2,2))/scl; /*1 + 2*cos(ang)*/
  if(trace<-1) trace = -1; else if(trace > 3) trace = 3;
  sinhalf = sqrt(3 - trace) / 2;		/* sin(angle/2) */

  axis.x[0] = Tij(1,2) - Tij(2,1);
  axis.x[1] = Tij(2,0) - Tij(0,2);
  axis.x[2] = Tij(0,1) - Tij(1,0);
  if(trace < -.25) {
    /* Angle near pi; sin(angle) is small, so use cos-related elements */
    float c = (trace-1)/2;	/* cos(angle) */
    float v = 1-c;		/* versine(angle) */

    if(Tij(0,0) > c+.5) {		/* large x component */
	axis.x[0] = sqrt((Tij(0,0)-c)/v) * (axis.x[0]<0 ? -1 : 1);
	axis.x[1] = (Tij(0,1)+Tij(1,0)) / (2*v*axis.x[0]);
	axis.x[2] = (Tij(0,2)+Tij(2,0)) / (2*v*axis.x[0]);

    } else if(Tij(1,1) > c+.5) {	/* large Y component */
	axis.x[1] = sqrt((Tij(1,1)-c)/v) * (axis.x[1]<0 ? -1 : 1);
	axis.x[0] = (Tij(0,1)+Tij(1,0)) / (2*v*axis.x[1]);
	axis.x[2] = (Tij(2,1)+Tij(1,2)) / (2*v*axis.x[1]);

    } else if(Tij(2,2) > c+.5) {	/* large Z component */
	axis.x[2] = sqrt((Tij(2,2)-c)/v) * (axis.x[2]<0 ? -1 : 1);
	axis.x[0] = (Tij(0,2)+Tij(2,0)) / (2*v*axis.x[2]);
	axis.x[1] = (Tij(2,1)+Tij(1,2)) / (2*v*axis.x[2]);
    } else {
	int i;
	fprintf(stderr, "Hey, tfm2quat() got a non-rotation matrix!\n");
	fprintf(stderr, "Check this out:\n");
	for(i=0;i<4;i++)
	  fprintf(stderr, "%12.8g %12.8g %12.8g %12.8g\n", Tij(i,0), Tij(i,1), Tij(i,2), Tij(i,3));
    }
  }
  mag = vlength(&axis);
  if(!finite(mag)) {
    fprintf(stderr, "Yikes, tfm2quat() yields NaN?\n");
  }
  /* The imaginary part is a vector pointing along the axis of rotation,
   * of magnitude sin(angle/2).  So normalize & scale the axis,
   * but don't fail if its magnitude was zero (i.e. no rotation).
   */
  vscale( iquat, mag==0 ? 0 : sinhalf/mag, &axis );
  return (float)sqrt(1 + trace) * .5f;
}

void quat2tfm( Matrix *dst, CONST Point *iquat )
{
  Point axis;
  float s, c, v, coshalf;
  int i, j;
  float sinhalf = vlength(iquat);

  mcopy( dst, &Tidentity );
  if(sinhalf == 0)
    return;
  if(sinhalf > 1) {
    fprintf(stderr, "quat2tfm: Yikes, clamping quat to length 1 (was 1+%g)\n", sinhalf-1);
    sinhalf = 1;
  }

  vscale(&axis, 1/sinhalf, iquat);

  coshalf = sqrt(1 - sinhalf*sinhalf);
  s = 2*sinhalf*coshalf;	/* sin(angle) */
  v = 2*sinhalf*sinhalf;	/* versine: 1 - cos(angle) */
  c = 1 - v;			/* cos(angle) */

  for(i = 0; i < 3; i++) {
    for(j = 0; j < i; j++)
	dst->m[4*i+j] = dst->m[4*j+i] = axis.x[i]*axis.x[j]*v;
    dst->m[4*i+i] = axis.x[i]*axis.x[i]*v + c;
  }
  dst->m[4*0+1] += axis.x[2]*s;  dst->m[4*1+0] -= axis.x[2]*s;
  dst->m[4*2+0] += axis.x[1]*s;  dst->m[4*0+2] -= axis.x[1]*s;
  dst->m[4*1+2] += axis.x[0]*s;  dst->m[4*2+1] -= axis.x[0]*s;
}

float qdist( CONST Point *q1, CONST Point *q2 ) {
  Point nq2;
  float s, sneg;
  vscale(&nq2, -1, q2);
  s = vdist(q1,q2);
  sneg = vdist(q1, &nq2);
  return (s < sneg) ? s : sneg;
}

float tdist( CONST Matrix *t1, CONST Matrix *t2 ) {
  float s = 0;
  int i;
  for(i=0; i<12; i++)
    s += (t1->m[i]-t2->m[i])*(t1->m[i]-t2->m[i]);
  return (float)sqrt(s);
}

void rot2tfm( Matrix *dst, float degrees, CONST Point *gaxis )
{
  Point axis;
  int i, j;
  float c, v, s;

  *dst = Tidentity;
  if(degrees == 0 || gaxis == NULL || vunit( &axis, gaxis ) == 0) {
    return;
  }
  c = (float)cos( degrees * (M_PI/180) );
  s = (float)sin( degrees * (M_PI/180) );
  v = 1 - c;

  for(i = 0; i < 3; i++) {
    for(j = 0; j < i; j++)
	dst->m[4*i+j] = dst->m[4*j+i] = axis.x[i]*axis.x[j]*v;
    dst->m[4*i+i] = axis.x[i]*axis.x[i]*v + c;
  }
  dst->m[4*0+1] += axis.x[2]*s;  dst->m[4*1+0] -= axis.x[2]*s;
  dst->m[4*2+0] += axis.x[1]*s;  dst->m[4*0+2] -= axis.x[1]*s;
  dst->m[4*1+2] += axis.x[0]*s;  dst->m[4*2+1] -= axis.x[0]*s;
}

void rot2quat( Point *iquat, float degrees, CONST Point *axis )
{
  vunit( iquat, axis );
  vscale( iquat, sin( degrees * (M_PI/360.) ), iquat );
}

void quat_lerp( Point *qdst, float frac, CONST Point *qfrom, CONST Point *qto ) {
  Point dst;
  Point tto = *qto;
  float s, rdst;
  float ifrom2 = VDOT(qfrom,qfrom);
  float rfrom = ifrom2>1 ? 0 : (float)sqrt(1 - ifrom2);	/* Real part */
  float ito2 = VDOT(&tto,&tto);
  float rto = ito2>1 ? 0 : sqrt(1 - ito2);
  float dot = VDOT(qfrom,&tto);
  if(dot < 0) {
	/* quaternions are in opposite hemispheres: negate "tto" */
	rto = -rto;
	vscale( &tto, -1, &tto );
  }
  /* Use linear interpolation between the quaternions.  This isn't right,
   * but shouldn't be far off if they don't differ by much.
   */
  rdst = (1-frac)*rfrom + frac*rto;
  vlerp( &dst, frac, qfrom, &tto );
  s = 1/sqrt(rdst*rdst + VDOT(&dst,&dst));
  if(!finite(s)) {
    fprintf(stderr, "Yeow!\n");
  }
  if(rdst < 0) s = -s;
  vscale( qdst, s, &dst );
}

/*
 * Decompose the rotation & translation part of a camera-to-world matrix
 * (which may also include some *uniform* scaling)
 * into a product of three rotations:
 *
 *  Rotation(c2w) = rot('z', aer[2]) * rot('x', aer[1]) * rot('y', aer[0])
 *
 * Angles are returned in *degrees*, not radians!
 * aer[] stands for Azimuth, Elevation, and Roll, in that order.
 * aer[0] ~ y-rotation (closest to world)
 * aer[1] ~ x-rotation
 * aer[2] ~ z-rotation (closest to camera)
 */
void tfm2xyzaer( Point *xyz, float aer[3], CONST Matrix *c2w )
{
  float sx, cx;
  Point xrow = *(Point *)&c2w->m[2*4+0];
  if(xyz != NULL)
    vgettranslation( xyz, c2w );
  if(aer == NULL)
    return;
  vunit( &xrow, &xrow );
  sx = -xrow.x[1];	/* normalized -m[2][1] */
  cx = (sx<-1 || sx>1) ? 0 : sqrt(1 - sx*sx);
  aer[1] = atan2( sx, cx ) * 180/M_PI;				/* xrot */
  if(cx < .001) {
    aer[0] = atan2( c2w->m[1*4+0], c2w->m[0*4+0] )
		* (aer[1] < 0 ? -180/M_PI : 180/M_PI);
    aer[2] = 0;
  } else {
    aer[0] = atan2( c2w->m[2*4+0], c2w->m[2*4+2] ) * 180/M_PI;	/* yrot */
    aer[2] = atan2( c2w->m[0*4+1], c2w->m[1*4+1] ) * 180/M_PI;	/* zrot */
  }
}

/*
 * Tcam2world = RotZ(aer[2]) * RotX(aer[1]) * RotY(aer[0]) * Translate(xyz)
 */
void xyzaer2tfm( Matrix *c2w, CONST Point *xyz, CONST float aer[3] )
{
  Matrix rx, ry, rz, t;
  if(aer == NULL) {
    *c2w = Tidentity;
  } else {
    mrotation( &ry, aer[0], 'y' );
    mrotation( &rx, aer[1], 'x' );
    mrotation( &rz, aer[2], 'z' );
    mmmul( &t, &rz, &rx );
    mmmul( c2w, &t,      &ry );
  }
  if(xyz != NULL)
    vsettranslation( c2w, xyz );
}

void vlerp( Point *dst, float frac, CONST Point *vfrom, CONST Point *vto )
{
  dst->x[0] = (1-frac)*vfrom->x[0] + frac*vto->x[0];
  dst->x[1] = (1-frac)*vfrom->x[1] + frac*vto->x[1];
  dst->x[2] = (1-frac)*vfrom->x[2] + frac*vto->x[2];
}

/* Linear combination: dst = sa*a + sb*b */
void vcomb( Point *dst, float sa, CONST Point *a, float sb, CONST Point *b )
{
  dst->x[0] = sa*a->x[0] + sb*b->x[0];
  dst->x[1] = sa*a->x[1] + sb*b->x[1];
  dst->x[2] = sa*a->x[2] + sb*b->x[2];
}