625 lines
15 KiB
C++
625 lines
15 KiB
C++
////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// 3d geometry classes - implements some 3d stuff
|
|
//
|
|
// g.j.hawkesford August 2003
|
|
//
|
|
// This program is released under the BSD license. See the file COPYING for details.
|
|
//
|
|
////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
#include "geometry.h"
|
|
using namespace geoff_geometry;
|
|
|
|
#ifdef PEPSDLL
|
|
#include "vdm.h"
|
|
#include "pepsdll.h"
|
|
#include "realds.h"
|
|
#endif
|
|
////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// matrix
|
|
////////////////////////////////////////////////////////////////////////////////////////////////
|
|
namespace geoff_geometry {
|
|
|
|
Matrix::Matrix(){
|
|
Unit();
|
|
}
|
|
Matrix::Matrix(double m[16]) {
|
|
memcpy(e, m, sizeof(e));
|
|
this->IsUnit();
|
|
this->IsMirrored();
|
|
}
|
|
|
|
Matrix::Matrix( const Matrix& m)
|
|
{
|
|
*this = m;
|
|
}
|
|
|
|
bool Matrix::operator==(const Matrix &m)const{
|
|
// m1 == m2
|
|
if(this->m_unit != m.m_unit || this->m_mirrored != m.m_mirrored) return false;
|
|
for(int i = 0; i < 16; i++)
|
|
if(FEQ(this->e[i], m.e[i], TIGHT_TOLERANCE) == false) return false;
|
|
return true;
|
|
}
|
|
|
|
#if 0
|
|
const Matrix& Matrix::operator=( Matrix &m) {
|
|
for(int i = 0; i < 16; i++) e[i] = m.e[i];
|
|
m_unit = m.m_unit;
|
|
m_mirrored = m.m_mirrored;
|
|
return *this;
|
|
}
|
|
#endif
|
|
void Matrix::Unit()
|
|
{
|
|
// homogenous matrix - set as unit matrix
|
|
memset(e, 0, sizeof(e));
|
|
e[0] = e[5] = e[10] = e[15] = 1;
|
|
m_unit = true;
|
|
m_mirrored = false;
|
|
}
|
|
|
|
void Matrix::Get(double* p) const
|
|
{
|
|
// copy the matrix
|
|
memcpy(p, e, sizeof(e));
|
|
}
|
|
void Matrix::Put(double* p)
|
|
{
|
|
// assign the matrix
|
|
memcpy(e, p, sizeof(e));
|
|
m_unit = false; // don't know
|
|
m_mirrored = -1; // don't know
|
|
|
|
}
|
|
void Matrix::Translate(double x, double y, double z)
|
|
{
|
|
// translation
|
|
e[3] += x;
|
|
e[7] += y;
|
|
e[11] += z;
|
|
m_unit = false;
|
|
}
|
|
|
|
void Matrix::Rotate(double angle, Vector3d *rotAxis) {
|
|
/// Rotation about rotAxis with angle
|
|
Rotate(sin(angle), cos(angle), rotAxis);
|
|
}
|
|
|
|
void Matrix::Rotate(double sinang, double cosang, Vector3d *rotAxis) {
|
|
/// Rotation about rotAxis with cp & dp
|
|
Matrix rotate;
|
|
double oneminusc = 1.0 - cosang;
|
|
|
|
rotate.e[0] = rotAxis->getx() * rotAxis->getx() * oneminusc + cosang;
|
|
rotate.e[1] = rotAxis->getx() * rotAxis->gety() * oneminusc - rotAxis->getz() * sinang;
|
|
rotate.e[2] = rotAxis->getx() * rotAxis->getz() * oneminusc + rotAxis->gety() * sinang;
|
|
|
|
rotate.e[4] = rotAxis->getx() * rotAxis->gety() * oneminusc + rotAxis->getz() * sinang;
|
|
rotate.e[5] = rotAxis->gety() * rotAxis->gety() * oneminusc + cosang;
|
|
rotate.e[6] = rotAxis->gety() * rotAxis->getz() * oneminusc - rotAxis->getx() * sinang;
|
|
|
|
rotate.e[8] = rotAxis->getx() * rotAxis->getz() * oneminusc - rotAxis->gety() * sinang;
|
|
rotate.e[9] = rotAxis->gety() * rotAxis->getz() * oneminusc + rotAxis->getx() * sinang;
|
|
rotate.e[10] = rotAxis->getz() * rotAxis->getz() * oneminusc + cosang;
|
|
Multiply(rotate); // concatinate rotation with this matrix
|
|
m_unit = false;
|
|
m_mirrored = -1; // don't know
|
|
}
|
|
|
|
|
|
void Matrix::Rotate(double angle, int Axis)
|
|
{ // Rotation (Axis 1 = x , 2 = y , 3 = z
|
|
Rotate(sin(angle), cos(angle), Axis);
|
|
}
|
|
|
|
void Matrix::Rotate(double sinang, double cosang, int Axis)
|
|
{ // Rotation (Axis 1 = x , 2 = y , 3 = z
|
|
Matrix rotate;
|
|
rotate.Unit();
|
|
|
|
switch(Axis)
|
|
{
|
|
case 1:
|
|
// about x axis
|
|
rotate.e[5] = rotate.e[10] = cosang;
|
|
rotate.e[6] = -sinang;
|
|
rotate.e[9] = sinang;
|
|
break;
|
|
case 2:
|
|
// about y axis
|
|
rotate.e[0] = rotate.e[10] = cosang;
|
|
rotate.e[2] = sinang;
|
|
rotate.e[8] = -sinang;
|
|
break;
|
|
case 3:
|
|
// about z axis
|
|
rotate.e[0] = rotate.e[5] = cosang;
|
|
rotate.e[1] = -sinang;
|
|
rotate.e[4] = sinang;
|
|
break;
|
|
}
|
|
Multiply(rotate); // concatinate rotation with this matrix
|
|
m_unit = false;
|
|
m_mirrored = -1; // don't know
|
|
}
|
|
|
|
void Matrix::Scale(double scale)
|
|
{
|
|
// add a scale
|
|
Scale(scale, scale, scale);
|
|
}
|
|
|
|
void Matrix::Scale(double scalex, double scaley, double scalez)
|
|
{
|
|
// add a scale
|
|
Matrix temp;
|
|
temp.Unit();
|
|
|
|
temp.e[0] = scalex;
|
|
temp.e[5] = scaley;
|
|
temp.e[10] = scalez;
|
|
Multiply(temp);
|
|
m_unit = false;
|
|
m_mirrored = -1; // don't know
|
|
}
|
|
void Matrix::Multiply(Matrix& m)
|
|
{
|
|
// multiply this by give matrix - concatinate
|
|
int i, k, l;
|
|
Matrix ret;
|
|
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
l = i - (k = (i % 4));
|
|
ret.e[i] = m.e[l] * e[k] + m.e[l+1] * e[k+4] + m.e[l+2] * e[k+8] + m.e[l+3] * e[k+12];
|
|
}
|
|
|
|
*this = ret;
|
|
this->IsUnit();
|
|
}
|
|
|
|
void Matrix::Transform(double p0[3], double p1[3]) const
|
|
{
|
|
// transform p0 thro' this matrix
|
|
if(m_unit)
|
|
memcpy(p1, p0, 3 * sizeof(double));
|
|
else {
|
|
p1[0] = p0[0] * e[0] + p0[1] * e[1] + p0[2] * e[2] + e[3];
|
|
p1[1] = p0[0] * e[4] + p0[1] * e[5] + p0[2] * e[6] + e[7];
|
|
p1[2] = p0[0] * e[8] + p0[1] * e[9] + p0[2] * e[10] + e[11];
|
|
}
|
|
}
|
|
void Matrix::Transform2d(double p0[2], double p1[2]) const
|
|
{
|
|
// transform p0 thro' this matrix (2d only)
|
|
if(m_unit)
|
|
memcpy(p1, p0, 2 * sizeof(double));
|
|
else {
|
|
p1[0] = p0[0] * e[0] + p0[1] * e[1] + e[3];
|
|
p1[1] = p0[0] * e[4] + p0[1] * e[5] + e[7];
|
|
}
|
|
}
|
|
|
|
void Matrix::Transform(double p0[3]) const
|
|
{
|
|
double p1[3];
|
|
if(!m_unit) {
|
|
Transform(p0, p1);
|
|
memcpy(p0, p1, 3 * sizeof(double));
|
|
}
|
|
}
|
|
|
|
int Matrix::IsMirrored()
|
|
{
|
|
// returns true if matrix has a mirror
|
|
if(m_unit)
|
|
m_mirrored = false;
|
|
else if(m_mirrored == -1) {
|
|
|
|
m_mirrored = ((e[0] * (e[5] * e[10] - e[6] * e[9])
|
|
- e[1] * (e[4] * e[10] - e[6] * e[8])
|
|
+ e[2] * (e[4] * e[9] - e[5] * e[8])) < 0);
|
|
}
|
|
return m_mirrored;
|
|
}
|
|
int Matrix::IsUnit() {
|
|
// returns true if unit matrix
|
|
for(int i = 0; i < 16; i++) {
|
|
if(i == 0 || i == 5 || i == 10 || i == 15) {
|
|
if(e[i] != 1) return m_unit = false;
|
|
}
|
|
else {
|
|
if(e[i] != 0) return m_unit = false;
|
|
}
|
|
}
|
|
m_mirrored = false;
|
|
return m_unit = true;
|
|
}
|
|
|
|
void Matrix::GetTranslate(double& x, double& y, double& z) const
|
|
{
|
|
// return translation
|
|
x = e[3];
|
|
y = e[7];
|
|
z = e[11];
|
|
}
|
|
void Matrix::GetScale(double& sx, double& sy, double& sz) const
|
|
{
|
|
// return the scale
|
|
if(m_unit) {
|
|
sx = sy = sz = 1;
|
|
}
|
|
else {
|
|
sx = sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]);
|
|
sy = sqrt(e[4] * e[4] + e[5] * e[5] + e[6] * e[6]);
|
|
sz = sqrt(e[8] * e[8] + e[9] * e[9] + e[10] * e[10]);
|
|
}
|
|
}
|
|
bool Matrix::GetScale(double& sx) const
|
|
{
|
|
// return a uniform scale (false if differential)
|
|
double sy, sz;
|
|
if(m_unit) {
|
|
sx = 1;
|
|
return true;
|
|
}
|
|
GetScale(sx, sy, sz);
|
|
return (fabs(fabs(sx) - fabs(sy)) < 0.000001)?true : false;
|
|
}
|
|
void Matrix::GetRotation(double& ax, double& ay, double& az) const
|
|
{
|
|
// return the rotations
|
|
if(m_unit) {
|
|
ax = ay = az = 0;
|
|
return;
|
|
}
|
|
double a; /* cos(bx) */
|
|
double b; /* sin(bx) */
|
|
double c; /* cos(by) */
|
|
double d; /* sin(by) */
|
|
double ee; /* cos(bz) */
|
|
double f; /* sin(bz) */
|
|
double sx, sy, sz;
|
|
GetScale(sx, sy, sz);
|
|
if(this->m_mirrored == -1) FAILURE(L"Don't know mirror - use IsMirrored method on object");
|
|
if(this->m_mirrored) sx = -sx;
|
|
|
|
// solve for d and decide case and solve for a, b, c, e and f
|
|
d = - e[8] / sz;
|
|
if((c = (1 - d) * (1 + d)) > 0.001)
|
|
{
|
|
// case 1
|
|
c = sqrt( c );
|
|
a = e[10] / sz / c;
|
|
b = e[9] / sz / c;
|
|
ee = e[0] / sx / c;
|
|
f = e[4] / sy / c;
|
|
}
|
|
else
|
|
{
|
|
// case 2
|
|
double coef;
|
|
double p, q;
|
|
|
|
d = ( d < 0 ) ? -1 : 1 ;
|
|
c = 0 ;
|
|
p = d * e[5] / sy - e[2] / sx;
|
|
q = d * e[6] / sy + e[1] / sx;
|
|
if((coef = sqrt( p * p + q * q )) > 0.001) {
|
|
a = q / coef;
|
|
b = p / coef;
|
|
ee = b;
|
|
f = -d * b;
|
|
}
|
|
else
|
|
{
|
|
/* dependent pairs */
|
|
a = e[5] / sy;
|
|
b = -e[6] / sy;
|
|
ee = 1 ;
|
|
f = 0 ;
|
|
}
|
|
}
|
|
|
|
// solve and return ax, ay and az
|
|
ax = atan2( b, a );
|
|
ay = atan2( d, c );
|
|
az = atan2( f, ee );
|
|
}
|
|
|
|
Matrix Matrix::Inverse()
|
|
{
|
|
// matrix inversion routine
|
|
|
|
// a is input matrix destroyed & replaced by inverse
|
|
// method used is gauss-jordan (ref ibm applications)
|
|
|
|
double hold , biga ;
|
|
int i , j , k , nk , kk , ij , iz ;
|
|
int ki , ji , jp , jk , kj , jq , jr , ik;
|
|
|
|
int n = 4; // 4 x 4 matrix only
|
|
Matrix a = *this;
|
|
int l[4], m[4];
|
|
|
|
if(a.m_unit) return a; // unit matrix
|
|
|
|
// search for largest element
|
|
nk = - n ;
|
|
for ( k = 0 ; k < n ; k++ ) {
|
|
nk += n ;
|
|
l [ k ] = m [ k ] = k ;
|
|
kk = nk + k ;
|
|
biga = a.e[ kk ] ;
|
|
|
|
for ( j = k ; j < n ; j++ ) {
|
|
iz = n * j ;
|
|
for ( i = k ; i < n ; i++ ) {
|
|
ij = iz + i ;
|
|
if ( fabs ( biga ) < fabs ( a.e[ ij ] ) ) {
|
|
biga = a.e[ ij ] ;
|
|
l[ k ] = i ;
|
|
m[ k ] = j ;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// interchange rows
|
|
j = l[ k ] ;
|
|
if ( j > k ) {
|
|
ki = k - n ;
|
|
|
|
for ( i = 0 ; i < n ; i++ ) {
|
|
ki += n ;
|
|
hold = - a.e[ ki ] ;
|
|
ji = ki - k + j ;
|
|
a.e[ ki ] = a.e[ ji ] ;
|
|
a.e[ ji ] = hold ;
|
|
}
|
|
}
|
|
|
|
// interchange columns
|
|
i = m[ k ] ;
|
|
if ( i > k ) {
|
|
jp = n * i ;
|
|
for ( j = 0 ; j < n ; j++ ) {
|
|
jk = nk + j ;
|
|
ji = jp + j ;
|
|
hold = - a.e[ jk ] ;
|
|
a.e[ jk ] = a.e[ ji ] ;
|
|
a.e[ ji ] = hold ;
|
|
}
|
|
}
|
|
|
|
// divide columns by minus pivot (value of pivot element is contained in biga)
|
|
if ( fabs ( biga ) < 1.0e-10 )FAILURE(getMessage(L"Singular Matrix - Inversion failure")); // singular matrix
|
|
|
|
for ( i = 0 ; i < n ; i++ ) {
|
|
if ( i != k ) {
|
|
ik = nk + i ;
|
|
a.e[ ik ] = - a.e[ ik ] /biga ;
|
|
}
|
|
}
|
|
|
|
// reduce matrix
|
|
for ( i = 0 ; i < n ; i++ ) {
|
|
ik = nk + i ;
|
|
hold = a.e[ ik ] ;
|
|
ij = i - n ;
|
|
|
|
for ( j = 0 ; j < n ; j++ ) {
|
|
ij = ij + n ;
|
|
if ( i != k && j != k ) {
|
|
kj = ij - i + k ;
|
|
a.e[ ij ] = hold * a.e[ kj ] + a.e[ ij ] ;
|
|
}
|
|
}
|
|
}
|
|
|
|
// divide row by pivot
|
|
kj = k - n ;
|
|
for ( j = 0 ; j < n ; j++ ) {
|
|
kj = kj + n ;
|
|
if ( j != k ) a.e[ kj] = a.e[ kj ] /biga ;
|
|
}
|
|
|
|
// replace pivot by reciprocal
|
|
a.e[ kk ] = 1 / biga ;
|
|
}
|
|
|
|
// final row and column interchange
|
|
k = n - 1 ;
|
|
|
|
while ( k > 0 ) {
|
|
i = l[ --k ] ;
|
|
if ( i > k ) {
|
|
jq = n * k ;
|
|
jr = n * i ;
|
|
|
|
for ( j = 0 ; j < n ; j++ ) {
|
|
jk = jq + j ;
|
|
hold = a.e[jk] ;
|
|
ji = jr + j ;
|
|
a.e[jk] = - a.e[ji] ;
|
|
a.e[ji] = hold ;
|
|
}
|
|
}
|
|
|
|
j = m[ k ] ;
|
|
if ( j > k ) {
|
|
ki = k - n ;
|
|
|
|
for ( i = 1 ; i <= n ; i ++ ) {
|
|
ki = ki + n ;
|
|
hold = a.e[ ki ] ;
|
|
ji = ki - k + j ;
|
|
a.e[ ki ] = - a.e[ ji ] ;
|
|
a.e[ ji ] = hold ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return a;
|
|
}
|
|
|
|
#ifdef PEPSDLL
|
|
void Matrix::ToPeps(int id)
|
|
{
|
|
int set = PepsVdmMake(id, VDM_MATRIX_TYPE , VDM_LOCAL);
|
|
if(set < 0) FAILURE(L"Failed to create Matrix VDM");
|
|
struct kgm_header pepsm;
|
|
|
|
Get(pepsm.matrix);
|
|
pepsm.off_rad = 0;
|
|
pepsm.off_dir = pepsm.origin_id = 0;
|
|
|
|
PepsVdmWriteTmx(set , &pepsm );
|
|
|
|
PepsVdmClose(set);
|
|
|
|
}
|
|
|
|
void Matrix::FromPeps(int id)
|
|
{
|
|
// if(id) {
|
|
int set = PepsVdmOpen(id, VDM_MATRIX_TYPE , VDM_READ_ONLY | VDM_LOCAL);
|
|
if(set < 0) FAILURE(L"Failed to open Matrix VDM");
|
|
|
|
struct kgm_header pepsm;
|
|
PepsVdmReadTmx(set , &pepsm);
|
|
memcpy(e, pepsm.matrix, sizeof(pepsm.matrix));
|
|
m_unit = true;
|
|
for(int i = 0; i < 16; i++) {
|
|
// copy over matrix and check for unit matrix
|
|
if(i == 0 || i == 5 || i == 10 || i == 15) {
|
|
if((e[i] = pepsm.matrix[i]) != 1) m_unit = false;
|
|
}
|
|
else {
|
|
if((e[i] = pepsm.matrix[i]) != 0) m_unit = false;
|
|
}
|
|
}
|
|
PepsVdmClose(set);
|
|
m_mirrored = IsMirrored();
|
|
// }
|
|
}
|
|
#endif
|
|
|
|
Matrix UnitMatrix; // a global unit matrix
|
|
|
|
|
|
// vector
|
|
Vector2d::Vector2d(const Vector3d &v){
|
|
if(FEQZ(v.getz())) FAILURE(L"Converting Vector3d to Vector2d illegal");
|
|
dx = v.getx();
|
|
dy = v.gety();
|
|
}
|
|
|
|
bool Vector2d::operator==(const Vector2d &v)const {
|
|
return FEQ(dx, v.getx(), 1.0e-06) && FEQ(dy, v.gety(), 1.0e-06);
|
|
}
|
|
|
|
void Vector2d::Transform(const Matrix& m) {
|
|
// transform vector
|
|
if(m.m_unit == false) {
|
|
double dxt = dx * m.e[0] + dy * m.e[1];
|
|
double dyt = dx * m.e[4] + dy * m.e[5];
|
|
dx = dxt;
|
|
dy = dyt;
|
|
}
|
|
this->normalise();
|
|
}
|
|
|
|
void Vector3d::Transform(const Matrix& m) {
|
|
// transform vector
|
|
if(m.m_unit == false) {
|
|
double dxt = dx * m.e[0] + dy * m.e[1] + dz * m.e[2];
|
|
double dyt = dx * m.e[4] + dy * m.e[5] + dz * m.e[6];
|
|
double dzt = dx * m.e[8] + dy * m.e[9] + dz * m.e[10];
|
|
dx = dxt;
|
|
dy = dyt;
|
|
dz = dzt;
|
|
}
|
|
this->normalise();
|
|
}
|
|
|
|
void Vector3d::arbitrary_axes(Vector3d& x, Vector3d& y){
|
|
// arbitrary axis algorithm - acad method of generating an arbitrary but
|
|
// consistant set of axes from a single normal ( z )
|
|
// arbitrary x & y axes
|
|
|
|
if ( ( fabs ( this->getx() ) < 1.0/64.0 ) && (fabs(this->gety()) < 1.0/64.0))
|
|
x = Y_VECTOR ^ *this;
|
|
else
|
|
x = Z_VECTOR ^ *this;
|
|
|
|
y = *this ^ x;
|
|
}
|
|
|
|
int Vector3d::setCartesianAxes(Vector3d& b, Vector3d& c) {
|
|
#define a *this
|
|
// computes a RH triad of Axes (Cartesian) starting from a (normalised)
|
|
// if a & b are perpendicular then c = a ^ b
|
|
// if a & c are perpendicular then b = c ^ a
|
|
// if neither are perpendicular to a, then return arbitrary axes from a
|
|
|
|
// calling sequence for RH cartesian
|
|
// x y z
|
|
// y z x
|
|
// z x y
|
|
if(a == NULL_VECTOR) FAILURE(L"SetAxes given a NULL Vector");
|
|
double epsilon = 1.0e-09;
|
|
bool bNull = (b == NULL_VECTOR);
|
|
bool cNull = (c == NULL_VECTOR);
|
|
bool abPerp = !bNull;
|
|
if(abPerp) abPerp = (fabs(a * b) < epsilon);
|
|
|
|
bool acPerp = !cNull;
|
|
if(acPerp) acPerp = (fabs(a * c) < epsilon);
|
|
|
|
if(abPerp) {
|
|
c = a ^ b;
|
|
return 1;
|
|
}
|
|
|
|
if(acPerp) {
|
|
b = c ^ a;
|
|
return 1;
|
|
}
|
|
|
|
arbitrary_axes(b, c);
|
|
b.normalise();
|
|
c.normalise();
|
|
return 2;
|
|
}
|
|
|
|
|
|
void Plane::Mirrored(Matrix* tmMirrored) {
|
|
// calculates a mirror transformation that mirrors 2d about plane
|
|
|
|
Point3d p1 = this->Near(Point3d(0.,0.,0.));
|
|
if(tmMirrored->m_unit == false) tmMirrored->Unit();
|
|
|
|
double nx = this->normal.getx();
|
|
double ny = this->normal.gety();
|
|
double nz = this->normal.getz();
|
|
|
|
// the translation
|
|
tmMirrored->e[ 3] = -2. * nx * this->d;
|
|
tmMirrored->e[ 7] = -2. * ny * this->d;
|
|
tmMirrored->e[11] = -2. * nz * this->d;
|
|
|
|
// the rest
|
|
tmMirrored->e[ 0] = 1. - 2. * nx * nx;
|
|
tmMirrored->e[ 5] = 1. - 2. * ny * ny;
|
|
tmMirrored->e[10] = 1. - 2. * nz * nz;
|
|
tmMirrored->e[ 1] = tmMirrored->e[ 4] = -2. * nx * ny;
|
|
tmMirrored->e[ 2] = tmMirrored->e[ 8] = -2. * nz * nx;
|
|
tmMirrored->e[ 6] = tmMirrored->e[ 9] = -2. * ny * nz;
|
|
|
|
tmMirrored->m_unit = false;
|
|
tmMirrored->m_mirrored = true;
|
|
}
|
|
}
|