Skip to content
Snippets Groups Projects
Commit 155ebbf6 authored by teuben's avatar teuben
Browse files

Initial revision

parent d10ab211
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 538 additions and 0 deletions
src/geometry.c 0 → 100644
+ 538
− 0
View file @ 155ebbf6
#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];
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Contact help@ncsa.illinois.edu with questions regarding this site. | All rights reserved. ©2024 Board of Trustees of the University of Illinois. | Web Privacy Notice