//-----------------------------------------------------------------------------

// Utility functions, mostly various kinds of vector math (working on real

// numbers, not working on quantities in the symbolic algebra system).

//

// Copyright 2008-2013 Jonathan Westhues.

//-----------------------------------------------------------------------------

#include "solvespace.h"

void SolveSpace::AssertFailure(const char *file, unsigned line, const char *function,

const char *condition, const char *message) {

std::string formattedMsg;

formattedMsg += ssprintf("File %s, line %u, function %s:\n", file, line, function);

formattedMsg += ssprintf("Assertion failed: %s.\n", condition);

formattedMsg += ssprintf("Message: %s.\n", message);

SolveSpace::Platform::FatalError(formattedMsg);

}

std::string SolveSpace::ssprintf(const char *fmt, ...)

{

va_list va;

va_start(va, fmt);

int size = vsnprintf(NULL, 0, fmt, va);

ssassert(size >= 0, "vsnprintf could not encode string");

va_end(va);

std::string result;

result.resize(size + 1);

va_start(va, fmt);

vsnprintf(&result[0], size + 1, fmt, va);

va_end(va);

result.resize(size);

return result;

}

char32_t utf8_iterator::operator*()

{

const uint8_t *it = (const uint8_t*) this->p;

char32_t result = *it;

if((result & 0x80) != 0) {

unsigned int mask = 0x40;

do {

result <<= 6;

unsigned int c = (*++it);

mask <<= 5;

result += c - 0x80;

} while((result & mask) != 0);

result &= mask - 1;

}

this->n = (const char*) (it + 1);

return result;

}

int64_t SolveSpace::GetMilliseconds()

{

auto timestamp = std::chrono::steady_clock::now().time_since_epoch();

return std::chrono::duration_cast<std::chrono::milliseconds>(timestamp).count();

}

void SolveSpace::MakeMatrix(double *mat,

double a11, double a12, double a13, double a14,

double a21, double a22, double a23, double a24,

double a31, double a32, double a33, double a34,

double a41, double a42, double a43, double a44)

{

mat[ 0] = a11;

mat[ 1] = a21;

mat[ 2] = a31;

mat[ 3] = a41;

mat[ 4] = a12;

mat[ 5] = a22;

mat[ 6] = a32;

mat[ 7] = a42;

mat[ 8] = a13;

mat[ 9] = a23;

mat[10] = a33;

mat[11] = a43;

mat[12] = a14;

mat[13] = a24;

mat[14] = a34;

mat[15] = a44;

}

void SolveSpace::MultMatrix(double *mata, double *matb, double *matr) {

for(int i = 0; i < 4; i++) {

for(int j = 0; j < 4; j++) {

double s = 0.0;

for(int k = 0; k < 4; k++) {

s += mata[k * 4 + j] * matb[i * 4 + k];

}

matr[i * 4 + j] = s;

}

}

}

//-----------------------------------------------------------------------------

// Format the string for our message box appropriately, and then display

// that string.

//-----------------------------------------------------------------------------

static void MessageBox(const char *fmt, va_list va, bool error,

std::function<void()> onDismiss = std::function<void()>())

{

#ifndef LIBRARY

va_list va_size;

va_copy(va_size, va);

int size = vsnprintf(NULL, 0, fmt, va_size);

ssassert(size >= 0, "vsnprintf could not encode string");

va_end(va_size);

std::string text;

text.resize(size);

vsnprintf(&text[0], size + 1, fmt, va);

// Split message text using a heuristic for better presentation.

size_t separatorAt = 0;

while(separatorAt != std::string::npos) {

size_t dotAt = text.find('.', separatorAt + 1),

colonAt = text.find(':', separatorAt + 1);

separatorAt = min(dotAt, colonAt);

if(separatorAt == std::string::npos ||

(separatorAt + 1 < text.size() && isspace(text[separatorAt + 1]))) {

break;

}

}

std::string message = text;

std::string description;

if(separatorAt != std::string::npos) {

message = text.substr(0, separatorAt + 1);

if(separatorAt + 1 < text.size()) {

description = text.substr(separatorAt + 1);

}

}

std::string::iterator it = description.begin();

while(isspace(*it)) it++;

description = description.substr(it - description.begin());

Platform::MessageDialogRef dialog = CreateMessageDialog(SS.GW.window);

if (!dialog) {

if (error) {

fprintf(stderr, "Error: %s\n", message.c_str());

} else {

fprintf(stderr, "Message: %s\n", message.c_str());

}

if(onDismiss) {

onDismiss();

}

return;

}

using Platform::MessageDialog;

if(error) {

dialog->SetType(MessageDialog::Type::ERROR);

} else {

dialog->SetType(MessageDialog::Type::INFORMATION);

}

dialog->SetTitle(error ? C_("title", "Error") : C_("title", "Message"));

dialog->SetMessage(message);

if(!description.empty()) {

dialog->SetDescription(description);

}

dialog->AddButton(C_("button", "&OK"), MessageDialog::Response::OK,

/*isDefault=*/true);

dialog->onResponse = [=](MessageDialog::Response _response) {

if(onDismiss) {

onDismiss();

}

};

dialog->ShowModal();

#endif

}

void SolveSpace::Error(const char *fmt, ...)

{

va_list f;

va_start(f, fmt);

MessageBox(fmt, f, /*error=*/true);

va_end(f);

}

void SolveSpace::Message(const char *fmt, ...)

{

va_list f;

va_start(f, fmt);

MessageBox(fmt, f, /*error=*/false);

va_end(f);

}

void SolveSpace::MessageAndRun(std::function<void()> onDismiss, const char *fmt, ...)

{

va_list f;

va_start(f, fmt);

MessageBox(fmt, f, /*error=*/false, onDismiss);

va_end(f);

}

//-----------------------------------------------------------------------------

// Solve a mostly banded matrix. In a given row, there are LEFT_OF_DIAG

// elements to the left of the diagonal element, and RIGHT_OF_DIAG elements to

// the right (so that the total band width is LEFT_OF_DIAG + RIGHT_OF_DIAG + 1).

// There also may be elements in the last two columns of any row. We solve

// without pivoting.

//-----------------------------------------------------------------------------

void BandedMatrix::Solve() {

int i, ip, j, jp;

double temp;

// Reduce the matrix to upper triangular form.

for(i = 0; i < n; i++) {

for(ip = i+1; ip < n && ip <= (i + LEFT_OF_DIAG); ip++) {

temp = A[ip][i]/A[i][i];

for(jp = i; jp < (n - 2) && jp <= (i + RIGHT_OF_DIAG); jp++) {

A[ip][jp] -= temp*(A[i][jp]);

}

A[ip][n-2] -= temp*(A[i][n-2]);

A[ip][n-1] -= temp*(A[i][n-1]);

B[ip] -= temp*B[i];

}

}

// And back-substitute.

for(i = n - 1; i >= 0; i--) {

temp = B[i];

if(i < n-1) temp -= X[n-1]*A[i][n-1];

if(i < n-2) temp -= X[n-2]*A[i][n-2];

for(j = min(n - 3, i + RIGHT_OF_DIAG); j > i; j--) {

temp -= X[j]*A[i][j];

}

X[i] = temp / A[i][i];

}

}

const Quaternion Quaternion::IDENTITY = { 1, 0, 0, 0 };

Quaternion Quaternion::From(double w, double vx, double vy, double vz) {

Quaternion q;

q.w = w;

q.vx = vx;

q.vy = vy;

q.vz = vz;

return q;

}

Quaternion Quaternion::From(hParam w, hParam vx, hParam vy, hParam vz) {

Quaternion q;

q.w = SK.GetParam(w )->val;

q.vx = SK.GetParam(vx)->val;

q.vy = SK.GetParam(vy)->val;

q.vz = SK.GetParam(vz)->val;

return q;

}

Quaternion Quaternion::From(Vector axis, double dtheta) {

Quaternion q;

double c = cos(dtheta / 2), s = sin(dtheta / 2);

axis = axis.WithMagnitude(s);

q.w = c;

q.vx = axis.x;

q.vy = axis.y;

q.vz = axis.z;

return q;

}

Quaternion Quaternion::From(Vector u, Vector v)

{

Vector n = u.Cross(v);

Quaternion q;

double s, tr = 1 + u.x + v.y + n.z;

if(tr > 1e-4) {

s = 2*sqrt(tr);

q.w = s/4;

q.vx = (v.z - n.y)/s;

q.vy = (n.x - u.z)/s;

q.vz = (u.y - v.x)/s;

} else {

if(u.x > v.y && u.x > n.z) {

s = 2*sqrt(1 + u.x - v.y - n.z);

q.w = (v.z - n.y)/s;

q.vx = s/4;

q.vy = (u.y + v.x)/s;

q.vz = (n.x + u.z)/s;

} else if(v.y > n.z) {

s = 2*sqrt(1 - u.x + v.y - n.z);

q.w = (n.x - u.z)/s;

q.vx = (u.y + v.x)/s;

q.vy = s/4;

q.vz = (v.z + n.y)/s;

} else {

s = 2*sqrt(1 - u.x - v.y + n.z);

q.w = (u.y - v.x)/s;

q.vx = (n.x + u.z)/s;

q.vy = (v.z + n.y)/s;

q.vz = s/4;

}

}

return q.WithMagnitude(1);

}

Quaternion Quaternion::Plus(Quaternion b) const {

Quaternion q;

q.w = w + b.w;

q.vx = vx + b.vx;

q.vy = vy + b.vy;

q.vz = vz + b.vz;

return q;

}

Quaternion Quaternion::Minus(Quaternion b) const {

Quaternion q;

q.w = w - b.w;

q.vx = vx - b.vx;

q.vy = vy - b.vy;

q.vz = vz - b.vz;

return q;

}

Quaternion Quaternion::ScaledBy(double s) const {

Quaternion q;

q.w = w*s;

q.vx = vx*s;

q.vy = vy*s;

q.vz = vz*s;

return q;

}

double Quaternion::Magnitude() const {

return sqrt(w*w + vx*vx + vy*vy + vz*vz);

}

Quaternion Quaternion::WithMagnitude(double s) const {

return ScaledBy(s/Magnitude());

}

Vector Quaternion::RotationU() const {

Vector v;

v.x = w*w + vx*vx - vy*vy - vz*vz;

v.y = 2*w *vz + 2*vx*vy;

v.z = 2*vx*vz - 2*w *vy;

return v;

}

Vector Quaternion::RotationV() const {

Vector v;

v.x = 2*vx*vy - 2*w*vz;

v.y = w*w - vx*vx + vy*vy - vz*vz;

v.z = 2*w*vx + 2*vy*vz;

return v;

}

Vector Quaternion::RotationN() const {

Vector v;

v.x = 2*w*vy + 2*vx*vz;

v.y = 2*vy*vz - 2*w*vx;

v.z = w*w - vx*vx - vy*vy + vz*vz;

return v;

}

Vector Quaternion::Rotate(Vector p) const {

// Express the point in the new basis

return (RotationU().ScaledBy(p.x)).Plus(

RotationV().ScaledBy(p.y)).Plus(

RotationN().ScaledBy(p.z));

}

Quaternion Quaternion::Inverse() const {

Quaternion r;

r.w = w;

r.vx = -vx;

r.vy = -vy;

r.vz = -vz;

return r.WithMagnitude(1); // not that the normalize should be reqd

}

Quaternion Quaternion::ToThe(double p) const {

// Avoid division by zero, or arccos of something not in its domain

if(w >= (1 - 1e-6)) {

return From(1, 0, 0, 0);

} else if(w <= (-1 + 1e-6)) {

return From(-1, 0, 0, 0);

}

Quaternion r;

Vector axis = Vector::From(vx, vy, vz);

double theta = acos(w); // okay, since magnitude is 1, so -1 <= w <= 1

theta *= p;

r.w = cos(theta);

axis = axis.WithMagnitude(sin(theta));

r.vx = axis.x;

r.vy = axis.y;

r.vz = axis.z;

return r;

}

Quaternion Quaternion::Times(Quaternion b) const {

double sa = w, sb = b.w;

Vector va = { vx, vy, vz };

Vector vb = { b.vx, b.vy, b.vz };

Quaternion r;

r.w = sa*sb - va.Dot(vb);

Vector vr = vb.ScaledBy(sa).Plus(

va.ScaledBy(sb).Plus(

va.Cross(vb)));

r.vx = vr.x;

r.vy = vr.y;

r.vz = vr.z;

return r;

}

Quaternion Quaternion::Mirror() const {

Vector u = RotationU(),

v = RotationV();

u = u.ScaledBy(-1);

v = v.ScaledBy(-1);

return Quaternion::From(u, v);

}

Vector Vector::From(double x, double y, double z) {

Vector v;

v.x = x; v.y = y; v.z = z;

return v;

}

Vector Vector::From(hParam x, hParam y, hParam z) {

Vector v;

v.x = SK.GetParam(x)->val;

v.y = SK.GetParam(y)->val;

v.z = SK.GetParam(z)->val;

return v;

}

bool Vector::EqualsExactly(Vector v) const {

return EXACT(x == v.x &&

y == v.y &&

z == v.z);

}

Vector Vector::Plus(Vector b) const {

Vector r;

r.x = x + b.x;

r.y = y + b.y;

r.z = z + b.z;

return r;

}

Vector Vector::Minus(Vector b) const {

Vector r;

r.x = x - b.x;

r.y = y - b.y;

r.z = z - b.z;

return r;

}

Vector Vector::Negated() const {

Vector r;

r.x = -x;

r.y = -y;

r.z = -z;

return r;

}

Vector Vector::Cross(Vector b) const {

Vector r;

r.x = -(z*b.y) + (y*b.z);

r.y = (z*b.x) - (x*b.z);

r.z = -(y*b.x) + (x*b.y);

return r;

}

double Vector::Dot(Vector b) const {

return (x*b.x + y*b.y + z*b.z);

}

double Vector::DirectionCosineWith(Vector b) const {

Vector a = this->WithMagnitude(1);

b = b.WithMagnitude(1);

return a.Dot(b);

}

Vector Vector::Normal(int which) const {

Vector n;

// Arbitrarily choose one vector that's normal to us, pivoting

// appropriately.

double xa = fabs(x), ya = fabs(y), za = fabs(z);

if(this->Equals(Vector::From(0, 0, 1))) {

// Make DXFs exported in the XY plane work nicely...

n = Vector::From(1, 0, 0);

} else if(xa < ya && xa < za) {

n.x = 0;

n.y = z;

n.z = -y;

} else if(ya < za) {

n.x = -z;

n.y = 0;

n.z = x;

} else {

n.x = y;

n.y = -x;

n.z = 0;

}

if(which == 0) {

// That's the vector we return.

} else if(which == 1) {

n = this->Cross(n);

} else ssassert(false, "Unexpected vector normal index");

n = n.WithMagnitude(1);

return n;

}

Vector Vector::RotatedAbout(Vector orig, Vector axis, double theta) const {

Vector r = this->Minus(orig);

r = r.RotatedAbout(axis, theta);

return r.Plus(orig);

}

Vector Vector::RotatedAbout(Vector axis, double theta) const {

double c = cos(theta);

double s = sin(theta);

axis = axis.WithMagnitude(1);

Vector r;

r.x = (x)*(c + (1 - c)*(axis.x)*(axis.x)) +

(y)*((1 - c)*(axis.x)*(axis.y) - s*(axis.z)) +

(z)*((1 - c)*(axis.x)*(axis.z) + s*(axis.y));

r.y = (x)*((1 - c)*(axis.y)*(axis.x) + s*(axis.z)) +

(y)*(c + (1 - c)*(axis.y)*(axis.y)) +

(z)*((1 - c)*(axis.y)*(axis.z) - s*(axis.x));

r.z = (x)*((1 - c)*(axis.z)*(axis.x) - s*(axis.y)) +

(y)*((1 - c)*(axis.z)*(axis.y) + s*(axis.x)) +

(z)*(c + (1 - c)*(axis.z)*(axis.z));

return r;

}

Vector Vector::DotInToCsys(Vector u, Vector v, Vector n) const {

Vector r = {

this->Dot(u),

this->Dot(v),

this->Dot(n)

};

return r;

}

Vector Vector::ScaleOutOfCsys(Vector u, Vector v, Vector n) const {

Vector r = u.ScaledBy(x).Plus(

v.ScaledBy(y).Plus(

n.ScaledBy(z)));

return r;

}

Vector Vector::InPerspective(Vector u, Vector v, Vector n,

Vector origin, double cameraTan) const

{

Vector r = this->Minus(origin);

r = r.DotInToCsys(u, v, n);

// yes, minus; we are assuming a csys where u cross v equals n, backwards

// from the display stuff

double w = (1 - r.z*cameraTan);

r = r.ScaledBy(1/w);

return r;

}

double Vector::DistanceToLine(Vector p0, Vector dp) const {

double m = dp.Magnitude();

return ((this->Minus(p0)).Cross(dp)).Magnitude() / m;

}

double Vector::DistanceToPlane(Vector normal, Vector origin) const {

return this->Dot(normal) - origin.Dot(normal);

}

bool Vector::OnLineSegment(Vector a, Vector b, double tol) const {

if(this->Equals(a, tol) || this->Equals(b, tol)) return true;

Vector d = b.Minus(a);

double m = d.MagSquared();

double distsq = ((this->Minus(a)).Cross(d)).MagSquared() / m;

if(distsq >= tol*tol) return false;

double t = (this->Minus(a)).DivProjected(d);

// On-endpoint already tested

if(t < 0 || t > 1) return false;

return true;

}

Vector Vector::ClosestPointOnLine(Vector p0, Vector dp) const {

dp = dp.WithMagnitude(1);

// this, p0, and (p0+dp) define a plane; the min distance is in

// that plane, so calculate its normal

Vector pn = (this->Minus(p0)).Cross(dp);

// The minimum distance line is in that plane, perpendicular

// to the line

Vector n = pn.Cross(dp);

// Calculate the actual distance

double d = (dp.Cross(p0.Minus(*this))).Magnitude();

return this->Plus(n.WithMagnitude(d));

}

double Vector::MagSquared() const {

return x*x + y*y + z*z;

}

double Vector::Magnitude() const {

return sqrt(x*x + y*y + z*z);

}

Vector Vector::ScaledBy(double v) const {

Vector r;

r.x = x * v;

r.y = y * v;

r.z = z * v;

return r;

}

Vector Vector::WithMagnitude(double v) const {

double m = Magnitude();

if(EXACT(m == 0)) {

// We can do a zero vector with zero magnitude, but not any other cases.

if(fabs(v) > 1e-100) {

dbp("Vector::WithMagnitude(%g) of zero vector!", v);

}

return From(0, 0, 0);

} else {

return ScaledBy(v/m);

}

}

Vector Vector::ProjectVectorInto(hEntity wrkpl) const {

EntityBase *w = SK.GetEntity(wrkpl);

Vector u = w->Normal()->NormalU();

Vector v = w->Normal()->NormalV();

double up = this->Dot(u);

double vp = this->Dot(v);

return (u.ScaledBy(up)).Plus(v.ScaledBy(vp));

}

Vector Vector::ProjectInto(hEntity wrkpl) const {

EntityBase *w = SK.GetEntity(wrkpl);

Vector p0 = w->WorkplaneGetOffset();

Vector f = this->Minus(p0);

return p0.Plus(f.ProjectVectorInto(wrkpl));

}

Point2d Vector::Project2d(Vector u, Vector v) const {

Point2d p;

p.x = this->Dot(u);

p.y = this->Dot(v);

return p;

}

Point2d Vector::ProjectXy() const {

Point2d p;

p.x = x;

p.y = y;

return p;

}

Vector4 Vector::Project4d() const {

return Vector4::From(1, x, y, z);

}

double Vector::DivProjected(Vector delta) const {

return (x*delta.x + y*delta.y + z*delta.z)

/ (delta.x*delta.x + delta.y*delta.y + delta.z*delta.z);

}

Vector Vector::ClosestOrtho() const {

double mx = fabs(x), my = fabs(y), mz = fabs(z);

if(mx > my && mx > mz) {

return From((x > 0) ? 1 : -1, 0, 0);

} else if(my > mz) {

return From(0, (y > 0) ? 1 : -1, 0);

} else {

return From(0, 0, (z > 0) ? 1 : -1);

}

}

Vector Vector::ClampWithin(double minv, double maxv) const {

Vector ret = *this;

if(ret.x < minv) ret.x = minv;

if(ret.y < minv) ret.y = minv;

if(ret.z < minv) ret.z = minv;

if(ret.x > maxv) ret.x = maxv;

if(ret.y > maxv) ret.y = maxv;

if(ret.z > maxv) ret.z = maxv;

return ret;

}

void Vector::MakeMaxMin(Vector *maxv, Vector *minv) const {

maxv->x = max(maxv->x, x);

maxv->y = max(maxv->y, y);

maxv->z = max(maxv->z, z);

minv->x = min(minv->x, x);

minv->y = min(minv->y, y);

minv->z = min(minv->z, z);

}

bool Vector::OutsideAndNotOn(Vector maxv, Vector minv) const {

return (x > maxv.x + LENGTH_EPS) || (x < minv.x - LENGTH_EPS) ||

(y > maxv.y + LENGTH_EPS) || (y < minv.y - LENGTH_EPS) ||

(z > maxv.z + LENGTH_EPS) || (z < minv.z - LENGTH_EPS);

}

bool Vector::BoundingBoxesDisjoint(Vector amax, Vector amin,

Vector bmax, Vector bmin)

{

int i;

for(i = 0; i < 3; i++) {

if(amax.Element(i) < bmin.Element(i) - LENGTH_EPS) return true;

if(amin.Element(i) > bmax.Element(i) + LENGTH_EPS) return true;

}

return false;

}

bool Vector::BoundingBoxIntersectsLine(Vector amax, Vector amin,

Vector p0, Vector p1, bool asSegment)

{

Vector dp = p1.Minus(p0);

double lp = dp.Magnitude();

dp = dp.ScaledBy(1.0/lp);

int i, a;

for(i = 0; i < 3; i++) {

int j = WRAP(i+1, 3), k = WRAP(i+2, 3);

if(lp*fabs(dp.Element(i)) < LENGTH_EPS) continue; // parallel to plane

for(a = 0; a < 2; a++) {

double d = (a == 0) ? amax.Element(i) : amin.Element(i);

// n dot (p0 + t*dp) = d

// (n dot p0) + t * (n dot dp) = d

double t = (d - p0.Element(i)) / dp.Element(i);

Vector p = p0.Plus(dp.ScaledBy(t));

if(asSegment && (t < -LENGTH_EPS || t > (lp+LENGTH_EPS))) continue;

if(p.Element(j) > amax.Element(j) + LENGTH_EPS) continue;

if(p.Element(k) > amax.Element(k) + LENGTH_EPS) continue;

if(p.Element(j) < amin.Element(j) - LENGTH_EPS) continue;

if(p.Element(k) < amin.Element(k) - LENGTH_EPS) continue;

return true;

}

}

return false;

}

Vector Vector::AtIntersectionOfPlanes(Vector n1, double d1,

Vector n2, double d2)

{

double det = (n1.Dot(n1))*(n2.Dot(n2)) -

(n1.Dot(n2))*(n1.Dot(n2));

double c1 = (d1*n2.Dot(n2) - d2*n1.Dot(n2))/det;

double c2 = (d2*n1.Dot(n1) - d1*n1.Dot(n2))/det;

return (n1.ScaledBy(c1)).Plus(n2.ScaledBy(c2));

}

void Vector::ClosestPointBetweenLines(Vector a0, Vector da,

Vector b0, Vector db,

double *ta, double *tb)

{

// Make a semi-orthogonal coordinate system from those directions;

// note that dna and dnb need not be perpendicular.

Vector dn = da.Cross(db); // normal to both

Vector dna = dn.Cross(da); // normal to da

Vector dnb = dn.Cross(db); // normal to db

// At the intersection of the lines

// a0 + pa*da = b0 + pb*db (where pa, pb are scalar params)

// So dot this equation against dna and dnb to get two equations

// to solve for da and db

*tb = ((a0.Minus(b0)).Dot(dna))/(db.Dot(dna));

*ta = -((a0.Minus(b0)).Dot(dnb))/(da.Dot(dnb));

}

Vector Vector::AtIntersectionOfLines(Vector a0, Vector a1,

Vector b0, Vector b1,

bool *skew,

double *parama, double *paramb)

{

Vector da = a1.Minus(a0), db = b1.Minus(b0);

double pa, pb;

Vector::ClosestPointBetweenLines(a0, da, b0, db, &pa, &pb);

if(parama) *parama = pa;

if(paramb) *paramb = pb;

// And from either of those, we get the intersection point.

Vector pi = a0.Plus(da.ScaledBy(pa));

if(skew) {

// Check if the intersection points on each line are actually

// coincident...

if(pi.Equals(b0.Plus(db.ScaledBy(pb)))) {

*skew = false;

} else {

*skew = true;

}

}

return pi;

}

Vector Vector::AtIntersectionOfPlaneAndLine(Vector n, double d,

Vector p0, Vector p1,

bool *parallel)

{

Vector dp = p1.Minus(p0);

if(fabs(n.Dot(dp)) < LENGTH_EPS) {

if(parallel) *parallel = true;

return Vector::From(0, 0, 0);

}

if(parallel) *parallel = false;

// n dot (p0 + t*dp) = d

// (n dot p0) + t * (n dot dp) = d

double t = (d - n.Dot(p0)) / (n.Dot(dp));

return p0.Plus(dp.ScaledBy(t));

}

static double det2(double a1, double b1,

double a2, double b2)

{

return (a1*b2) - (b1*a2);

}

static double det3(double a1, double b1, double c1,

double a2, double b2, double c2,

double a3, double b3, double c3)

{

return a1*det2(b2, c2, b3, c3) -

b1*det2(a2, c2, a3, c3) +

c1*det2(a2, b2, a3, b3);

}

Vector Vector::AtIntersectionOfPlanes(Vector na, double da,

Vector nb, double db,

Vector nc, double dc,

bool *parallel)

{

double det = det3(na.x, na.y, na.z,

nb.x, nb.y, nb.z,

nc.x, nc.y, nc.z);

if(fabs(det) < 1e-10) { // arbitrary tolerance, not so good

*parallel = true;

return Vector::From(0, 0, 0);

}

*parallel = false;

double detx = det3(da, na.y, na.z,

db, nb.y, nb.z,

dc, nc.y, nc.z);

double dety = det3(na.x, da, na.z,

nb.x, db, nb.z,

nc.x, dc, nc.z);

double detz = det3(na.x, na.y, da,

nb.x, nb.y, db,

nc.x, nc.y, dc );

return Vector::From(detx/det, dety/det, detz/det);

}

size_t VectorHash::operator()(const Vector &v) const {

const size_t size = (size_t)pow(std::numeric_limits<size_t>::max(), 1.0 / 3.0) - 1;

const double eps = 4.0 * LENGTH_EPS;

double x = fabs(v.x) / eps;

double y = fabs(v.y) / eps;

double z = fabs(v.y) / eps;

size_t xs = size_t(fmod(x, (double)size));

size_t ys = size_t(fmod(y, (double)size));

size_t zs = size_t(fmod(z, (double)size));

return (zs * size + ys) * size + xs;

}

bool VectorPred::operator()(Vector a, Vector b) const {

return a.Equals(b, LENGTH_EPS);

}

Vector4 Vector4::From(double w, double x, double y, double z) {

Vector4 ret;

ret.w = w;

ret.x = x;

ret.y = y;

ret.z = z;

return ret;

}

Vector4 Vector4::From(double w, Vector v) {

return Vector4::From(w, w*v.x, w*v.y, w*v.z);

}

Vector4 Vector4::Blend(Vector4 a, Vector4 b, double t) {

return (a.ScaledBy(1 - t)).Plus(b.ScaledBy(t));

}

Vector4 Vector4::Plus(Vector4 b) const {

return Vector4::From(w + b.w, x + b.x, y + b.y, z + b.z);

}

Vector4 Vector4::Minus(Vector4 b) const {

return Vector4::From(w - b.w, x - b.x, y - b.y, z - b.z);

}

Vector4 Vector4::ScaledBy(double s) const {

return Vector4::From(w*s, x*s, y*s, z*s);

}

Vector Vector4::PerspectiveProject() const {

return Vector::From(x / w, y / w, z / w);

}

Point2d Point2d::From(double x, double y) {

return { x, y };

}

Point2d Point2d::FromPolar(double r, double a) {

return { r * cos(a), r * sin(a) };

}

double Point2d::Angle() const {

double a = atan2(y, x);

return M_PI + remainder(a - M_PI, 2 * M_PI);

}

double Point2d::AngleTo(const Point2d &p) const {

return p.Minus(*this).Angle();

}

Point2d Point2d::Plus(const Point2d &b) const {

return { x + b.x, y + b.y };

}

Point2d Point2d::Minus(const Point2d &b) const {

return { x - b.x, y - b.y };

}

Point2d Point2d::ScaledBy(double s) const {

return { x * s, y * s };

}

double Point2d::DivProjected(Point2d delta) const {

return (x*delta.x + y*delta.y) / (delta.x*delta.x + delta.y*delta.y);

}

double Point2d::MagSquared() const {

return x*x + y*y;

}

double Point2d::Magnitude() const {

return sqrt(x*x + y*y);