3D geometry
This commit is contained in:
rsc
parent
46f79934b7
commit
d1e9002f81
7 changed files with 890 additions and 0 deletions
215
src/libgeometry/arith3.c
Normal file
215
src/libgeometry/arith3.c
Normal file
|
|
@ -0,0 +1,215 @@
|
|||
#include <u.h>
|
||||
#include <libc.h>
|
||||
#include <draw.h>
|
||||
#include <geometry.h>
|
||||
/*
|
||||
* Routines whose names end in 3 work on points in Affine 3-space.
|
||||
* They ignore w in all arguments and produce w=1 in all results.
|
||||
* Routines whose names end in 4 work on points in Projective 3-space.
|
||||
*/
|
||||
Point3 add3(Point3 a, Point3 b){
|
||||
a.x+=b.x;
|
||||
a.y+=b.y;
|
||||
a.z+=b.z;
|
||||
a.w=1.;
|
||||
return a;
|
||||
}
|
||||
Point3 sub3(Point3 a, Point3 b){
|
||||
a.x-=b.x;
|
||||
a.y-=b.y;
|
||||
a.z-=b.z;
|
||||
a.w=1.;
|
||||
return a;
|
||||
}
|
||||
Point3 neg3(Point3 a){
|
||||
a.x=-a.x;
|
||||
a.y=-a.y;
|
||||
a.z=-a.z;
|
||||
a.w=1.;
|
||||
return a;
|
||||
}
|
||||
Point3 div3(Point3 a, double b){
|
||||
a.x/=b;
|
||||
a.y/=b;
|
||||
a.z/=b;
|
||||
a.w=1.;
|
||||
return a;
|
||||
}
|
||||
Point3 mul3(Point3 a, double b){
|
||||
a.x*=b;
|
||||
a.y*=b;
|
||||
a.z*=b;
|
||||
a.w=1.;
|
||||
return a;
|
||||
}
|
||||
int eqpt3(Point3 p, Point3 q){
|
||||
return p.x==q.x && p.y==q.y && p.z==q.z;
|
||||
}
|
||||
/*
|
||||
* Are these points closer than eps, in a relative sense
|
||||
*/
|
||||
int closept3(Point3 p, Point3 q, double eps){
|
||||
return 2.*dist3(p, q)<eps*(len3(p)+len3(q));
|
||||
}
|
||||
double dot3(Point3 p, Point3 q){
|
||||
return p.x*q.x+p.y*q.y+p.z*q.z;
|
||||
}
|
||||
Point3 cross3(Point3 p, Point3 q){
|
||||
Point3 r;
|
||||
r.x=p.y*q.z-p.z*q.y;
|
||||
r.y=p.z*q.x-p.x*q.z;
|
||||
r.z=p.x*q.y-p.y*q.x;
|
||||
r.w=1.;
|
||||
return r;
|
||||
}
|
||||
double len3(Point3 p){
|
||||
return sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
|
||||
}
|
||||
double dist3(Point3 p, Point3 q){
|
||||
p.x-=q.x;
|
||||
p.y-=q.y;
|
||||
p.z-=q.z;
|
||||
return sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
|
||||
}
|
||||
Point3 unit3(Point3 p){
|
||||
double len=sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
|
||||
p.x/=len;
|
||||
p.y/=len;
|
||||
p.z/=len;
|
||||
p.w=1.;
|
||||
return p;
|
||||
}
|
||||
Point3 midpt3(Point3 p, Point3 q){
|
||||
p.x=.5*(p.x+q.x);
|
||||
p.y=.5*(p.y+q.y);
|
||||
p.z=.5*(p.z+q.z);
|
||||
p.w=1.;
|
||||
return p;
|
||||
}
|
||||
Point3 lerp3(Point3 p, Point3 q, double alpha){
|
||||
p.x+=(q.x-p.x)*alpha;
|
||||
p.y+=(q.y-p.y)*alpha;
|
||||
p.z+=(q.z-p.z)*alpha;
|
||||
p.w=1.;
|
||||
return p;
|
||||
}
|
||||
/*
|
||||
* Reflect point p in the line joining p0 and p1
|
||||
*/
|
||||
Point3 reflect3(Point3 p, Point3 p0, Point3 p1){
|
||||
Point3 a, b;
|
||||
a=sub3(p, p0);
|
||||
b=sub3(p1, p0);
|
||||
return add3(a, mul3(b, 2*dot3(a, b)/dot3(b, b)));
|
||||
}
|
||||
/*
|
||||
* Return the nearest point on segment [p0,p1] to point testp
|
||||
*/
|
||||
Point3 nearseg3(Point3 p0, Point3 p1, Point3 testp){
|
||||
double num, den;
|
||||
Point3 q, r;
|
||||
q=sub3(p1, p0);
|
||||
r=sub3(testp, p0);
|
||||
num=dot3(q, r);;
|
||||
if(num<=0) return p0;
|
||||
den=dot3(q, q);
|
||||
if(num>=den) return p1;
|
||||
return add3(p0, mul3(q, num/den));
|
||||
}
|
||||
/*
|
||||
* distance from point p to segment [p0,p1]
|
||||
*/
|
||||
#define SMALL 1e-8 /* what should this value be? */
|
||||
double pldist3(Point3 p, Point3 p0, Point3 p1){
|
||||
Point3 d, e;
|
||||
double dd, de, dsq;
|
||||
d=sub3(p1, p0);
|
||||
e=sub3(p, p0);
|
||||
dd=dot3(d, d);
|
||||
de=dot3(d, e);
|
||||
if(dd<SMALL*SMALL) return len3(e);
|
||||
dsq=dot3(e, e)-de*de/dd;
|
||||
if(dsq<SMALL*SMALL) return 0;
|
||||
return sqrt(dsq);
|
||||
}
|
||||
/*
|
||||
* vdiv3(a, b) is the magnitude of the projection of a onto b
|
||||
* measured in units of the length of b.
|
||||
* vrem3(a, b) is the component of a perpendicular to b.
|
||||
*/
|
||||
double vdiv3(Point3 a, Point3 b){
|
||||
return (a.x*b.x+a.y*b.y+a.z*b.z)/(b.x*b.x+b.y*b.y+b.z*b.z);
|
||||
}
|
||||
Point3 vrem3(Point3 a, Point3 b){
|
||||
double quo=(a.x*b.x+a.y*b.y+a.z*b.z)/(b.x*b.x+b.y*b.y+b.z*b.z);
|
||||
a.x-=b.x*quo;
|
||||
a.y-=b.y*quo;
|
||||
a.z-=b.z*quo;
|
||||
a.w=1.;
|
||||
return a;
|
||||
}
|
||||
/*
|
||||
* Compute face (plane) with given normal, containing a given point
|
||||
*/
|
||||
Point3 pn2f3(Point3 p, Point3 n){
|
||||
n.w=-dot3(p, n);
|
||||
return n;
|
||||
}
|
||||
/*
|
||||
* Compute face containing three points
|
||||
*/
|
||||
Point3 ppp2f3(Point3 p0, Point3 p1, Point3 p2){
|
||||
Point3 p01, p02;
|
||||
p01=sub3(p1, p0);
|
||||
p02=sub3(p2, p0);
|
||||
return pn2f3(p0, cross3(p01, p02));
|
||||
}
|
||||
/*
|
||||
* Compute point common to three faces.
|
||||
* Cramer's rule, yuk.
|
||||
*/
|
||||
Point3 fff2p3(Point3 f0, Point3 f1, Point3 f2){
|
||||
double det;
|
||||
Point3 p;
|
||||
det=dot3(f0, cross3(f1, f2));
|
||||
if(fabs(det)<SMALL){ /* parallel planes, bogus answer */
|
||||
p.x=0.;
|
||||
p.y=0.;
|
||||
p.z=0.;
|
||||
p.w=0.;
|
||||
return p;
|
||||
}
|
||||
p.x=(f0.w*(f2.y*f1.z-f1.y*f2.z)
|
||||
+f1.w*(f0.y*f2.z-f2.y*f0.z)+f2.w*(f1.y*f0.z-f0.y*f1.z))/det;
|
||||
p.y=(f0.w*(f2.z*f1.x-f1.z*f2.x)
|
||||
+f1.w*(f0.z*f2.x-f2.z*f0.x)+f2.w*(f1.z*f0.x-f0.z*f1.x))/det;
|
||||
p.z=(f0.w*(f2.x*f1.y-f1.x*f2.y)
|
||||
+f1.w*(f0.x*f2.y-f2.x*f0.y)+f2.w*(f1.x*f0.y-f0.x*f1.y))/det;
|
||||
p.w=1.;
|
||||
return p;
|
||||
}
|
||||
/*
|
||||
* pdiv4 does perspective division to convert a projective point to affine coordinates.
|
||||
*/
|
||||
Point3 pdiv4(Point3 a){
|
||||
if(a.w==0) return a;
|
||||
a.x/=a.w;
|
||||
a.y/=a.w;
|
||||
a.z/=a.w;
|
||||
a.w=1.;
|
||||
return a;
|
||||
}
|
||||
Point3 add4(Point3 a, Point3 b){
|
||||
a.x+=b.x;
|
||||
a.y+=b.y;
|
||||
a.z+=b.z;
|
||||
a.w+=b.w;
|
||||
return a;
|
||||
}
|
||||
Point3 sub4(Point3 a, Point3 b){
|
||||
a.x-=b.x;
|
||||
a.y-=b.y;
|
||||
a.z-=b.z;
|
||||
a.w-=b.w;
|
||||
return a;
|
||||
}
|
||||
106
src/libgeometry/matrix.c
Normal file
106
src/libgeometry/matrix.c
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
/*
|
||||
* ident(m) store identity matrix in m
|
||||
* matmul(a, b) matrix multiply a*=b
|
||||
* matmulr(a, b) matrix multiply a=b*a
|
||||
* determinant(m) returns det(m)
|
||||
* adjoint(m, minv) minv=adj(m)
|
||||
* invertmat(m, minv) invert matrix m, result in minv, returns det(m)
|
||||
* if m is singular, minv=adj(m)
|
||||
*/
|
||||
#include <u.h>
|
||||
#include <libc.h>
|
||||
#include <draw.h>
|
||||
#include <geometry.h>
|
||||
void ident(Matrix m){
|
||||
register double *s=&m[0][0];
|
||||
*s++=1;*s++=0;*s++=0;*s++=0;
|
||||
*s++=0;*s++=1;*s++=0;*s++=0;
|
||||
*s++=0;*s++=0;*s++=1;*s++=0;
|
||||
*s++=0;*s++=0;*s++=0;*s=1;
|
||||
}
|
||||
void matmul(Matrix a, Matrix b){
|
||||
int i, j, k;
|
||||
double sum;
|
||||
Matrix tmp;
|
||||
for(i=0;i!=4;i++) for(j=0;j!=4;j++){
|
||||
sum=0;
|
||||
for(k=0;k!=4;k++)
|
||||
sum+=a[i][k]*b[k][j];
|
||||
tmp[i][j]=sum;
|
||||
}
|
||||
for(i=0;i!=4;i++) for(j=0;j!=4;j++)
|
||||
a[i][j]=tmp[i][j];
|
||||
}
|
||||
void matmulr(Matrix a, Matrix b){
|
||||
int i, j, k;
|
||||
double sum;
|
||||
Matrix tmp;
|
||||
for(i=0;i!=4;i++) for(j=0;j!=4;j++){
|
||||
sum=0;
|
||||
for(k=0;k!=4;k++)
|
||||
sum+=b[i][k]*a[k][j];
|
||||
tmp[i][j]=sum;
|
||||
}
|
||||
for(i=0;i!=4;i++) for(j=0;j!=4;j++)
|
||||
a[i][j]=tmp[i][j];
|
||||
}
|
||||
/*
|
||||
* Return det(m)
|
||||
*/
|
||||
double determinant(Matrix m){
|
||||
return m[0][0]*(m[1][1]*(m[2][2]*m[3][3]-m[2][3]*m[3][2])+
|
||||
m[1][2]*(m[2][3]*m[3][1]-m[2][1]*m[3][3])+
|
||||
m[1][3]*(m[2][1]*m[3][2]-m[2][2]*m[3][1]))
|
||||
-m[0][1]*(m[1][0]*(m[2][2]*m[3][3]-m[2][3]*m[3][2])+
|
||||
m[1][2]*(m[2][3]*m[3][0]-m[2][0]*m[3][3])+
|
||||
m[1][3]*(m[2][0]*m[3][2]-m[2][2]*m[3][0]))
|
||||
+m[0][2]*(m[1][0]*(m[2][1]*m[3][3]-m[2][3]*m[3][1])+
|
||||
m[1][1]*(m[2][3]*m[3][0]-m[2][0]*m[3][3])+
|
||||
m[1][3]*(m[2][0]*m[3][1]-m[2][1]*m[3][0]))
|
||||
-m[0][3]*(m[1][0]*(m[2][1]*m[3][2]-m[2][2]*m[3][1])+
|
||||
m[1][1]*(m[2][2]*m[3][0]-m[2][0]*m[3][2])+
|
||||
m[1][2]*(m[2][0]*m[3][1]-m[2][1]*m[3][0]));
|
||||
}
|
||||
/*
|
||||
* Store the adjoint (matrix of cofactors) of m in madj.
|
||||
* Works fine even if m and madj are the same matrix.
|
||||
*/
|
||||
void adjoint(Matrix m, Matrix madj){
|
||||
double m00=m[0][0], m01=m[0][1], m02=m[0][2], m03=m[0][3];
|
||||
double m10=m[1][0], m11=m[1][1], m12=m[1][2], m13=m[1][3];
|
||||
double m20=m[2][0], m21=m[2][1], m22=m[2][2], m23=m[2][3];
|
||||
double m30=m[3][0], m31=m[3][1], m32=m[3][2], m33=m[3][3];
|
||||
madj[0][0]=m11*(m22*m33-m23*m32)+m21*(m13*m32-m12*m33)+m31*(m12*m23-m13*m22);
|
||||
madj[0][1]=m01*(m23*m32-m22*m33)+m21*(m02*m33-m03*m32)+m31*(m03*m22-m02*m23);
|
||||
madj[0][2]=m01*(m12*m33-m13*m32)+m11*(m03*m32-m02*m33)+m31*(m02*m13-m03*m12);
|
||||
madj[0][3]=m01*(m13*m22-m12*m23)+m11*(m02*m23-m03*m22)+m21*(m03*m12-m02*m13);
|
||||
madj[1][0]=m10*(m23*m32-m22*m33)+m20*(m12*m33-m13*m32)+m30*(m13*m22-m12*m23);
|
||||
madj[1][1]=m00*(m22*m33-m23*m32)+m20*(m03*m32-m02*m33)+m30*(m02*m23-m03*m22);
|
||||
madj[1][2]=m00*(m13*m32-m12*m33)+m10*(m02*m33-m03*m32)+m30*(m03*m12-m02*m13);
|
||||
madj[1][3]=m00*(m12*m23-m13*m22)+m10*(m03*m22-m02*m23)+m20*(m02*m13-m03*m12);
|
||||
madj[2][0]=m10*(m21*m33-m23*m31)+m20*(m13*m31-m11*m33)+m30*(m11*m23-m13*m21);
|
||||
madj[2][1]=m00*(m23*m31-m21*m33)+m20*(m01*m33-m03*m31)+m30*(m03*m21-m01*m23);
|
||||
madj[2][2]=m00*(m11*m33-m13*m31)+m10*(m03*m31-m01*m33)+m30*(m01*m13-m03*m11);
|
||||
madj[2][3]=m00*(m13*m21-m11*m23)+m10*(m01*m23-m03*m21)+m20*(m03*m11-m01*m13);
|
||||
madj[3][0]=m10*(m22*m31-m21*m32)+m20*(m11*m32-m12*m31)+m30*(m12*m21-m11*m22);
|
||||
madj[3][1]=m00*(m21*m32-m22*m31)+m20*(m02*m31-m01*m32)+m30*(m01*m22-m02*m21);
|
||||
madj[3][2]=m00*(m12*m31-m11*m32)+m10*(m01*m32-m02*m31)+m30*(m02*m11-m01*m12);
|
||||
madj[3][3]=m00*(m11*m22-m12*m21)+m10*(m02*m21-m01*m22)+m20*(m01*m12-m02*m11);
|
||||
}
|
||||
/*
|
||||
* Store the inverse of m in minv.
|
||||
* If m is singular, minv is instead its adjoint.
|
||||
* Returns det(m).
|
||||
* Works fine even if m and minv are the same matrix.
|
||||
*/
|
||||
double invertmat(Matrix m, Matrix minv){
|
||||
double d, dinv;
|
||||
int i, j;
|
||||
d=determinant(m);
|
||||
adjoint(m, minv);
|
||||
if(d!=0.){
|
||||
dinv=1./d;
|
||||
for(i=0;i!=4;i++) for(j=0;j!=4;j++) minv[i][j]*=dinv;
|
||||
}
|
||||
return d;
|
||||
}
|
||||
17
src/libgeometry/mkfile
Normal file
17
src/libgeometry/mkfile
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
<$PLAN9/src/mkhdr
|
||||
|
||||
LIB=libgeometry.a
|
||||
OFILES=\
|
||||
arith3.$O\
|
||||
matrix.$O\
|
||||
qball.$O\
|
||||
quaternion.$O\
|
||||
transform.$O\
|
||||
tstack.$O\
|
||||
|
||||
HFILES=$PLAN9/include/geometry.h
|
||||
|
||||
<$PLAN9/src/mksyslib
|
||||
|
||||
listing:V:
|
||||
pr mkfile $HFILES $CFILES|lp -du
|
||||
66
src/libgeometry/qball.c
Normal file
66
src/libgeometry/qball.c
Normal file
|
|
@ -0,0 +1,66 @@
|
|||
/*
|
||||
* Ken Shoemake's Quaternion rotation controller
|
||||
*/
|
||||
#include <u.h>
|
||||
#include <libc.h>
|
||||
#include <draw.h>
|
||||
#include <stdio.h>
|
||||
#include <event.h>
|
||||
#include <geometry.h>
|
||||
#define BORDER 4
|
||||
static Point ctlcen; /* center of qball */
|
||||
static int ctlrad; /* radius of qball */
|
||||
static Quaternion *axis; /* constraint plane orientation, 0 if none */
|
||||
/*
|
||||
* Convert a mouse point into a unit quaternion, flattening if
|
||||
* constrained to a particular plane.
|
||||
*/
|
||||
static Quaternion mouseq(Point p){
|
||||
double qx=(double)(p.x-ctlcen.x)/ctlrad;
|
||||
double qy=(double)(p.y-ctlcen.y)/ctlrad;
|
||||
double rsq=qx*qx+qy*qy;
|
||||
double l;
|
||||
Quaternion q;
|
||||
if(rsq>1){
|
||||
rsq=sqrt(rsq);
|
||||
q.r=0.;
|
||||
q.i=qx/rsq;
|
||||
q.j=qy/rsq;
|
||||
q.k=0.;
|
||||
}
|
||||
else{
|
||||
q.r=0.;
|
||||
q.i=qx;
|
||||
q.j=qy;
|
||||
q.k=sqrt(1.-rsq);
|
||||
}
|
||||
if(axis){
|
||||
l=q.i*axis->i+q.j*axis->j+q.k*axis->k;
|
||||
q.i-=l*axis->i;
|
||||
q.j-=l*axis->j;
|
||||
q.k-=l*axis->k;
|
||||
l=sqrt(q.i*q.i+q.j*q.j+q.k*q.k);
|
||||
if(l!=0.){
|
||||
q.i/=l;
|
||||
q.j/=l;
|
||||
q.k/=l;
|
||||
}
|
||||
}
|
||||
return q;
|
||||
}
|
||||
void qball(Rectangle r, Mouse *m, Quaternion *result, void (*redraw)(void), Quaternion *ap){
|
||||
Quaternion q, down;
|
||||
Point rad;
|
||||
axis=ap;
|
||||
ctlcen=divpt(addpt(r.min, r.max), 2);
|
||||
rad=divpt(subpt(r.max, r.min), 2);
|
||||
ctlrad=(rad.x<rad.y?rad.x:rad.y)-BORDER;
|
||||
down=qinv(mouseq(m->xy));
|
||||
q=*result;
|
||||
for(;;){
|
||||
*m=emouse();
|
||||
if(!m->buttons) break;
|
||||
*result=qmul(q, qmul(down, mouseq(m->xy)));
|
||||
(*redraw)();
|
||||
}
|
||||
}
|
||||
242
src/libgeometry/quaternion.c
Normal file
242
src/libgeometry/quaternion.c
Normal file
|
|
@ -0,0 +1,242 @@
|
|||
/*
|
||||
* Quaternion arithmetic:
|
||||
* qadd(q, r) returns q+r
|
||||
* qsub(q, r) returns q-r
|
||||
* qneg(q) returns -q
|
||||
* qmul(q, r) returns q*r
|
||||
* qdiv(q, r) returns q/r, can divide check.
|
||||
* qinv(q) returns 1/q, can divide check.
|
||||
* double qlen(p) returns modulus of p
|
||||
* qunit(q) returns a unit quaternion parallel to q
|
||||
* The following only work on unit quaternions and rotation matrices:
|
||||
* slerp(q, r, a) returns q*(r*q^-1)^a
|
||||
* qmid(q, r) slerp(q, r, .5)
|
||||
* qsqrt(q) qmid(q, (Quaternion){1,0,0,0})
|
||||
* qtom(m, q) converts a unit quaternion q into a rotation matrix m
|
||||
* mtoq(m) returns a quaternion equivalent to a rotation matrix m
|
||||
*/
|
||||
#include <u.h>
|
||||
#include <libc.h>
|
||||
#include <draw.h>
|
||||
#include <geometry.h>
|
||||
void qtom(Matrix m, Quaternion q){
|
||||
#ifndef new
|
||||
m[0][0]=1-2*(q.j*q.j+q.k*q.k);
|
||||
m[0][1]=2*(q.i*q.j+q.r*q.k);
|
||||
m[0][2]=2*(q.i*q.k-q.r*q.j);
|
||||
m[0][3]=0;
|
||||
m[1][0]=2*(q.i*q.j-q.r*q.k);
|
||||
m[1][1]=1-2*(q.i*q.i+q.k*q.k);
|
||||
m[1][2]=2*(q.j*q.k+q.r*q.i);
|
||||
m[1][3]=0;
|
||||
m[2][0]=2*(q.i*q.k+q.r*q.j);
|
||||
m[2][1]=2*(q.j*q.k-q.r*q.i);
|
||||
m[2][2]=1-2*(q.i*q.i+q.j*q.j);
|
||||
m[2][3]=0;
|
||||
m[3][0]=0;
|
||||
m[3][1]=0;
|
||||
m[3][2]=0;
|
||||
m[3][3]=1;
|
||||
#else
|
||||
/*
|
||||
* Transcribed from Ken Shoemake's new code -- not known to work
|
||||
*/
|
||||
double Nq = q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k;
|
||||
double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
|
||||
double xs = q.i*s, ys = q.j*s, zs = q.k*s;
|
||||
double wx = q.r*xs, wy = q.r*ys, wz = q.r*zs;
|
||||
double xx = q.i*xs, xy = q.i*ys, xz = q.i*zs;
|
||||
double yy = q.j*ys, yz = q.j*zs, zz = q.k*zs;
|
||||
m[0][0] = 1.0 - (yy + zz); m[1][0] = xy + wz; m[2][0] = xz - wy;
|
||||
m[0][1] = xy - wz; m[1][1] = 1.0 - (xx + zz); m[2][1] = yz + wx;
|
||||
m[0][2] = xz + wy; m[1][2] = yz - wx; m[2][2] = 1.0 - (xx + yy);
|
||||
m[0][3] = m[1][3] = m[2][3] = m[3][0] = m[3][1] = m[3][2] = 0.0;
|
||||
m[3][3] = 1.0;
|
||||
#endif
|
||||
}
|
||||
Quaternion mtoq(Matrix mat){
|
||||
#ifndef new
|
||||
#define EPS 1.387778780781445675529539585113525e-17 /* 2^-56 */
|
||||
double t;
|
||||
Quaternion q;
|
||||
q.r=0.;
|
||||
q.i=0.;
|
||||
q.j=0.;
|
||||
q.k=1.;
|
||||
if((t=.25*(1+mat[0][0]+mat[1][1]+mat[2][2]))>EPS){
|
||||
q.r=sqrt(t);
|
||||
t=4*q.r;
|
||||
q.i=(mat[1][2]-mat[2][1])/t;
|
||||
q.j=(mat[2][0]-mat[0][2])/t;
|
||||
q.k=(mat[0][1]-mat[1][0])/t;
|
||||
}
|
||||
else if((t=-.5*(mat[1][1]+mat[2][2]))>EPS){
|
||||
q.i=sqrt(t);
|
||||
t=2*q.i;
|
||||
q.j=mat[0][1]/t;
|
||||
q.k=mat[0][2]/t;
|
||||
}
|
||||
else if((t=.5*(1-mat[2][2]))>EPS){
|
||||
q.j=sqrt(t);
|
||||
q.k=mat[1][2]/(2*q.j);
|
||||
}
|
||||
return q;
|
||||
#else
|
||||
/*
|
||||
* Transcribed from Ken Shoemake's new code -- not known to work
|
||||
*/
|
||||
/* This algorithm avoids near-zero divides by looking for a large
|
||||
* component -- first r, then i, j, or k. When the trace is greater than zero,
|
||||
* |r| is greater than 1/2, which is as small as a largest component can be.
|
||||
* Otherwise, the largest diagonal entry corresponds to the largest of |i|,
|
||||
* |j|, or |k|, one of which must be larger than |r|, and at least 1/2.
|
||||
*/
|
||||
Quaternion qu;
|
||||
double tr, s;
|
||||
|
||||
tr = mat[0][0] + mat[1][1] + mat[2][2];
|
||||
if (tr >= 0.0) {
|
||||
s = sqrt(tr + mat[3][3]);
|
||||
qu.r = s*0.5;
|
||||
s = 0.5 / s;
|
||||
qu.i = (mat[2][1] - mat[1][2]) * s;
|
||||
qu.j = (mat[0][2] - mat[2][0]) * s;
|
||||
qu.k = (mat[1][0] - mat[0][1]) * s;
|
||||
}
|
||||
else {
|
||||
int i = 0;
|
||||
if (mat[1][1] > mat[0][0]) i = 1;
|
||||
if (mat[2][2] > mat[i][i]) i = 2;
|
||||
switch(i){
|
||||
case 0:
|
||||
s = sqrt( (mat[0][0] - (mat[1][1]+mat[2][2])) + mat[3][3] );
|
||||
qu.i = s*0.5;
|
||||
s = 0.5 / s;
|
||||
qu.j = (mat[0][1] + mat[1][0]) * s;
|
||||
qu.k = (mat[2][0] + mat[0][2]) * s;
|
||||
qu.r = (mat[2][1] - mat[1][2]) * s;
|
||||
break;
|
||||
case 1:
|
||||
s = sqrt( (mat[1][1] - (mat[2][2]+mat[0][0])) + mat[3][3] );
|
||||
qu.j = s*0.5;
|
||||
s = 0.5 / s;
|
||||
qu.k = (mat[1][2] + mat[2][1]) * s;
|
||||
qu.i = (mat[0][1] + mat[1][0]) * s;
|
||||
qu.r = (mat[0][2] - mat[2][0]) * s;
|
||||
break;
|
||||
case 2:
|
||||
s = sqrt( (mat[2][2] - (mat[0][0]+mat[1][1])) + mat[3][3] );
|
||||
qu.k = s*0.5;
|
||||
s = 0.5 / s;
|
||||
qu.i = (mat[2][0] + mat[0][2]) * s;
|
||||
qu.j = (mat[1][2] + mat[2][1]) * s;
|
||||
qu.r = (mat[1][0] - mat[0][1]) * s;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (mat[3][3] != 1.0){
|
||||
s=1/sqrt(mat[3][3]);
|
||||
qu.r*=s;
|
||||
qu.i*=s;
|
||||
qu.j*=s;
|
||||
qu.k*=s;
|
||||
}
|
||||
return (qu);
|
||||
#endif
|
||||
}
|
||||
Quaternion qadd(Quaternion q, Quaternion r){
|
||||
q.r+=r.r;
|
||||
q.i+=r.i;
|
||||
q.j+=r.j;
|
||||
q.k+=r.k;
|
||||
return q;
|
||||
}
|
||||
Quaternion qsub(Quaternion q, Quaternion r){
|
||||
q.r-=r.r;
|
||||
q.i-=r.i;
|
||||
q.j-=r.j;
|
||||
q.k-=r.k;
|
||||
return q;
|
||||
}
|
||||
Quaternion qneg(Quaternion q){
|
||||
q.r=-q.r;
|
||||
q.i=-q.i;
|
||||
q.j=-q.j;
|
||||
q.k=-q.k;
|
||||
return q;
|
||||
}
|
||||
Quaternion qmul(Quaternion q, Quaternion r){
|
||||
Quaternion s;
|
||||
s.r=q.r*r.r-q.i*r.i-q.j*r.j-q.k*r.k;
|
||||
s.i=q.r*r.i+r.r*q.i+q.j*r.k-q.k*r.j;
|
||||
s.j=q.r*r.j+r.r*q.j+q.k*r.i-q.i*r.k;
|
||||
s.k=q.r*r.k+r.r*q.k+q.i*r.j-q.j*r.i;
|
||||
return s;
|
||||
}
|
||||
Quaternion qdiv(Quaternion q, Quaternion r){
|
||||
return qmul(q, qinv(r));
|
||||
}
|
||||
Quaternion qunit(Quaternion q){
|
||||
double l=qlen(q);
|
||||
q.r/=l;
|
||||
q.i/=l;
|
||||
q.j/=l;
|
||||
q.k/=l;
|
||||
return q;
|
||||
}
|
||||
/*
|
||||
* Bug?: takes no action on divide check
|
||||
*/
|
||||
Quaternion qinv(Quaternion q){
|
||||
double l=q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k;
|
||||
q.r/=l;
|
||||
q.i=-q.i/l;
|
||||
q.j=-q.j/l;
|
||||
q.k=-q.k/l;
|
||||
return q;
|
||||
}
|
||||
double qlen(Quaternion p){
|
||||
return sqrt(p.r*p.r+p.i*p.i+p.j*p.j+p.k*p.k);
|
||||
}
|
||||
Quaternion slerp(Quaternion q, Quaternion r, double a){
|
||||
double u, v, ang, s;
|
||||
double dot=q.r*r.r+q.i*r.i+q.j*r.j+q.k*r.k;
|
||||
ang=dot<-1?PI:dot>1?0:acos(dot); /* acos gives NaN for dot slightly out of range */
|
||||
s=sin(ang);
|
||||
if(s==0) return ang<PI/2?q:r;
|
||||
u=sin((1-a)*ang)/s;
|
||||
v=sin(a*ang)/s;
|
||||
q.r=u*q.r+v*r.r;
|
||||
q.i=u*q.i+v*r.i;
|
||||
q.j=u*q.j+v*r.j;
|
||||
q.k=u*q.k+v*r.k;
|
||||
return q;
|
||||
}
|
||||
/*
|
||||
* Only works if qlen(q)==qlen(r)==1
|
||||
*/
|
||||
Quaternion qmid(Quaternion q, Quaternion r){
|
||||
double l;
|
||||
q=qadd(q, r);
|
||||
l=qlen(q);
|
||||
if(l<1e-12){
|
||||
q.r=r.i;
|
||||
q.i=-r.r;
|
||||
q.j=r.k;
|
||||
q.k=-r.j;
|
||||
}
|
||||
else{
|
||||
q.r/=l;
|
||||
q.i/=l;
|
||||
q.j/=l;
|
||||
q.k/=l;
|
||||
}
|
||||
return q;
|
||||
}
|
||||
/*
|
||||
* Only works if qlen(q)==1
|
||||
*/
|
||||
static Quaternion qident={1,0,0,0};
|
||||
Quaternion qsqrt(Quaternion q){
|
||||
return qmid(q, qident);
|
||||
}
|
||||
75
src/libgeometry/transform.c
Normal file
75
src/libgeometry/transform.c
Normal file
|
|
@ -0,0 +1,75 @@
|
|||
/*
|
||||
* The following routines transform points and planes from one space
|
||||
* to another. Points and planes are represented by their
|
||||
* homogeneous coordinates, stored in variables of type Point3.
|
||||
*/
|
||||
#include <u.h>
|
||||
#include <libc.h>
|
||||
#include <draw.h>
|
||||
#include <geometry.h>
|
||||
/*
|
||||
* Transform point p.
|
||||
*/
|
||||
Point3 xformpoint(Point3 p, Space *to, Space *from){
|
||||
Point3 q, r;
|
||||
register double *m;
|
||||
if(from){
|
||||
m=&from->t[0][0];
|
||||
q.x=*m++*p.x; q.x+=*m++*p.y; q.x+=*m++*p.z; q.x+=*m++*p.w;
|
||||
q.y=*m++*p.x; q.y+=*m++*p.y; q.y+=*m++*p.z; q.y+=*m++*p.w;
|
||||
q.z=*m++*p.x; q.z+=*m++*p.y; q.z+=*m++*p.z; q.z+=*m++*p.w;
|
||||
q.w=*m++*p.x; q.w+=*m++*p.y; q.w+=*m++*p.z; q.w+=*m *p.w;
|
||||
}
|
||||
else
|
||||
q=p;
|
||||
if(to){
|
||||
m=&to->tinv[0][0];
|
||||
r.x=*m++*q.x; r.x+=*m++*q.y; r.x+=*m++*q.z; r.x+=*m++*q.w;
|
||||
r.y=*m++*q.x; r.y+=*m++*q.y; r.y+=*m++*q.z; r.y+=*m++*q.w;
|
||||
r.z=*m++*q.x; r.z+=*m++*q.y; r.z+=*m++*q.z; r.z+=*m++*q.w;
|
||||
r.w=*m++*q.x; r.w+=*m++*q.y; r.w+=*m++*q.z; r.w+=*m *q.w;
|
||||
}
|
||||
else
|
||||
r=q;
|
||||
return r;
|
||||
}
|
||||
/*
|
||||
* Transform point p with perspective division.
|
||||
*/
|
||||
Point3 xformpointd(Point3 p, Space *to, Space *from){
|
||||
p=xformpoint(p, to, from);
|
||||
if(p.w!=0){
|
||||
p.x/=p.w;
|
||||
p.y/=p.w;
|
||||
p.z/=p.w;
|
||||
p.w=1;
|
||||
}
|
||||
return p;
|
||||
}
|
||||
/*
|
||||
* Transform plane p -- same as xformpoint, except multiply on the
|
||||
* other side by the inverse matrix.
|
||||
*/
|
||||
Point3 xformplane(Point3 p, Space *to, Space *from){
|
||||
Point3 q, r;
|
||||
register double *m;
|
||||
if(from){
|
||||
m=&from->tinv[0][0];
|
||||
q.x =*m++*p.x; q.y =*m++*p.x; q.z =*m++*p.x; q.w =*m++*p.x;
|
||||
q.x+=*m++*p.y; q.y+=*m++*p.y; q.z+=*m++*p.y; q.w+=*m++*p.y;
|
||||
q.x+=*m++*p.z; q.y+=*m++*p.z; q.z+=*m++*p.z; q.w+=*m++*p.z;
|
||||
q.x+=*m++*p.w; q.y+=*m++*p.w; q.z+=*m++*p.w; q.w+=*m *p.w;
|
||||
}
|
||||
else
|
||||
q=p;
|
||||
if(to){
|
||||
m=&to->t[0][0];
|
||||
r.x =*m++*q.x; r.y =*m++*q.x; r.z =*m++*q.x; r.w =*m++*q.x;
|
||||
r.x+=*m++*q.y; r.y+=*m++*q.y; r.z+=*m++*q.y; r.w+=*m++*q.y;
|
||||
r.x+=*m++*q.z; r.y+=*m++*q.z; r.z+=*m++*q.z; r.w+=*m++*q.z;
|
||||
r.x+=*m++*q.w; r.y+=*m++*q.w; r.z+=*m++*q.w; r.w+=*m *q.w;
|
||||
}
|
||||
else
|
||||
r=q;
|
||||
return r;
|
||||
}
|
||||
169
src/libgeometry/tstack.c
Normal file
169
src/libgeometry/tstack.c
Normal file
|
|
@ -0,0 +1,169 @@
|
|||
/*% cc -gpc %
|
||||
* These transformation routines maintain stacks of transformations
|
||||
* and their inverses.
|
||||
* t=pushmat(t) push matrix stack
|
||||
* t=popmat(t) pop matrix stack
|
||||
* rot(t, a, axis) multiply stack top by rotation
|
||||
* qrot(t, q) multiply stack top by rotation, q is unit quaternion
|
||||
* scale(t, x, y, z) multiply stack top by scale
|
||||
* move(t, x, y, z) multiply stack top by translation
|
||||
* xform(t, m) multiply stack top by m
|
||||
* ixform(t, m, inv) multiply stack top by m. inv is the inverse of m.
|
||||
* look(t, e, l, u) multiply stack top by viewing transformation
|
||||
* persp(t, fov, n, f) multiply stack top by perspective transformation
|
||||
* viewport(t, r, aspect)
|
||||
* multiply stack top by window->viewport transformation.
|
||||
*/
|
||||
#include <u.h>
|
||||
#include <libc.h>
|
||||
#include <draw.h>
|
||||
#include <geometry.h>
|
||||
Space *pushmat(Space *t){
|
||||
Space *v;
|
||||
v=malloc(sizeof(Space));
|
||||
if(t==0){
|
||||
ident(v->t);
|
||||
ident(v->tinv);
|
||||
}
|
||||
else
|
||||
*v=*t;
|
||||
v->next=t;
|
||||
return v;
|
||||
}
|
||||
Space *popmat(Space *t){
|
||||
Space *v;
|
||||
if(t==0) return 0;
|
||||
v=t->next;
|
||||
free(t);
|
||||
return v;
|
||||
}
|
||||
void rot(Space *t, double theta, int axis){
|
||||
double s=sin(radians(theta)), c=cos(radians(theta));
|
||||
Matrix m, inv;
|
||||
int i=(axis+1)%3, j=(axis+2)%3;
|
||||
ident(m);
|
||||
m[i][i] = c;
|
||||
m[i][j] = -s;
|
||||
m[j][i] = s;
|
||||
m[j][j] = c;
|
||||
ident(inv);
|
||||
inv[i][i] = c;
|
||||
inv[i][j] = s;
|
||||
inv[j][i] = -s;
|
||||
inv[j][j] = c;
|
||||
ixform(t, m, inv);
|
||||
}
|
||||
void qrot(Space *t, Quaternion q){
|
||||
Matrix m, inv;
|
||||
int i, j;
|
||||
qtom(m, q);
|
||||
for(i=0;i!=4;i++) for(j=0;j!=4;j++) inv[i][j]=m[j][i];
|
||||
ixform(t, m, inv);
|
||||
}
|
||||
void scale(Space *t, double x, double y, double z){
|
||||
Matrix m, inv;
|
||||
ident(m);
|
||||
m[0][0]=x;
|
||||
m[1][1]=y;
|
||||
m[2][2]=z;
|
||||
ident(inv);
|
||||
inv[0][0]=1/x;
|
||||
inv[1][1]=1/y;
|
||||
inv[2][2]=1/z;
|
||||
ixform(t, m, inv);
|
||||
}
|
||||
void move(Space *t, double x, double y, double z){
|
||||
Matrix m, inv;
|
||||
ident(m);
|
||||
m[0][3]=x;
|
||||
m[1][3]=y;
|
||||
m[2][3]=z;
|
||||
ident(inv);
|
||||
inv[0][3]=-x;
|
||||
inv[1][3]=-y;
|
||||
inv[2][3]=-z;
|
||||
ixform(t, m, inv);
|
||||
}
|
||||
void xform(Space *t, Matrix m){
|
||||
Matrix inv;
|
||||
if(invertmat(m, inv)==0) return;
|
||||
ixform(t, m, inv);
|
||||
}
|
||||
void ixform(Space *t, Matrix m, Matrix inv){
|
||||
matmul(t->t, m);
|
||||
matmulr(t->tinv, inv);
|
||||
}
|
||||
/*
|
||||
* multiply the top of the matrix stack by a view-pointing transformation
|
||||
* with the eyepoint at e, looking at point l, with u at the top of the screen.
|
||||
* The coordinate system is deemed to be right-handed.
|
||||
* The generated transformation transforms this view into a view from
|
||||
* the origin, looking in the positive y direction, with the z axis pointing up,
|
||||
* and x to the right.
|
||||
*/
|
||||
void look(Space *t, Point3 e, Point3 l, Point3 u){
|
||||
Matrix m, inv;
|
||||
Point3 r;
|
||||
l=unit3(sub3(l, e));
|
||||
u=unit3(vrem3(sub3(u, e), l));
|
||||
r=cross3(l, u);
|
||||
/* make the matrix to transform from (rlu) space to (xyz) space */
|
||||
ident(m);
|
||||
m[0][0]=r.x; m[0][1]=r.y; m[0][2]=r.z;
|
||||
m[1][0]=l.x; m[1][1]=l.y; m[1][2]=l.z;
|
||||
m[2][0]=u.x; m[2][1]=u.y; m[2][2]=u.z;
|
||||
ident(inv);
|
||||
inv[0][0]=r.x; inv[0][1]=l.x; inv[0][2]=u.x;
|
||||
inv[1][0]=r.y; inv[1][1]=l.y; inv[1][2]=u.y;
|
||||
inv[2][0]=r.z; inv[2][1]=l.z; inv[2][2]=u.z;
|
||||
ixform(t, m, inv);
|
||||
move(t, -e.x, -e.y, -e.z);
|
||||
}
|
||||
/*
|
||||
* generate a transformation that maps the frustum with apex at the origin,
|
||||
* apex angle=fov and clipping planes y=n and y=f into the double-unit cube.
|
||||
* plane y=n maps to y'=-1, y=f maps to y'=1
|
||||
*/
|
||||
int persp(Space *t, double fov, double n, double f){
|
||||
Matrix m;
|
||||
double z;
|
||||
if(n<=0 || f<=n || fov<=0 || 180<=fov) /* really need f!=n && sin(v)!=0 */
|
||||
return -1;
|
||||
z=1/tan(radians(fov)/2);
|
||||
m[0][0]=z; m[0][1]=0; m[0][2]=0; m[0][3]=0;
|
||||
m[1][0]=0; m[1][1]=(f+n)/(f-n); m[1][2]=0; m[1][3]=f*(1-m[1][1]);
|
||||
m[2][0]=0; m[2][1]=0; m[2][2]=z; m[2][3]=0;
|
||||
m[3][0]=0; m[3][1]=1; m[3][2]=0; m[3][3]=0;
|
||||
xform(t, m);
|
||||
return 0;
|
||||
}
|
||||
/*
|
||||
* Map the unit-cube window into the given screen viewport.
|
||||
* r has min at the top left, max just outside the lower right. Aspect is the
|
||||
* aspect ratio (dx/dy) of the viewport's pixels (not of the whole viewport!)
|
||||
* The whole window is transformed to fit centered inside the viewport with equal
|
||||
* slop on either top and bottom or left and right, depending on the viewport's
|
||||
* aspect ratio.
|
||||
* The window is viewed down the y axis, with x to the left and z up. The viewport
|
||||
* has x increasing to the right and y increasing down. The window's y coordinates
|
||||
* are mapped, unchanged, into the viewport's z coordinates.
|
||||
*/
|
||||
void viewport(Space *t, Rectangle r, double aspect){
|
||||
Matrix m;
|
||||
double xc, yc, wid, hgt, scale;
|
||||
xc=.5*(r.min.x+r.max.x);
|
||||
yc=.5*(r.min.y+r.max.y);
|
||||
wid=(r.max.x-r.min.x)*aspect;
|
||||
hgt=r.max.y-r.min.y;
|
||||
scale=.5*(wid<hgt?wid:hgt);
|
||||
ident(m);
|
||||
m[0][0]=scale;
|
||||
m[0][3]=xc;
|
||||
m[1][1]=0;
|
||||
m[1][2]=-scale;
|
||||
m[1][3]=yc;
|
||||
m[2][1]=1;
|
||||
m[2][2]=0;
|
||||
/* should get inverse by hand */
|
||||
xform(t, m);
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue