678 lines
16 KiB
C++
678 lines
16 KiB
C++
// SPDX-License-Identifier: BSD-3-Clause
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// 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)) {
|
|
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()
|
|
{
|
|
// homogeneous 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); // concatenate 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); // concatenate 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 - concatenate
|
|
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) { // unit matrix
|
|
return a;
|
|
}
|
|
|
|
// 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) {
|
|
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) {
|
|
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
|
|
// consistent 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) {
|
|
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;
|
|
}
|
|
} // namespace geoff_geometry
|