

//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  This file is part of CMSDK version 2.1.1.260726 (26 July 2026)
//_/_/  
//_/_/  developed by
//_/_/  
//_/_/  Communicative Machines Ltd. 
//_/_/    http://cmlabs.com/
//_/_/  Copyright (c) 2026
//_/_/  
//_/_/  in collaboration with 
//_/_/  
//_/_/  Icelandic Institute for Intelligent Machines ses
//_/_/    Menntavegur 1
//_/_/    101 Reykjavik
//_/_/    ICELAND
//_/_/    http://www.iiim.is/
//_/_/
//_/_/  and is released under the following
//_/_/  
//_/_/  EXTENDED BSD License
//_/_/
//_/_/  Redistribution and use in source and binary forms, with or without
//_/_/  modification, is permitted provided that the following conditions 
//_/_/  are met:
//_/_/
//_/_/  - Redistributions of source code must retain the above copyright 
//_/_/    and collaboration notice, this list of conditions and the following 
//_/_/    disclaimer.
//_/_/
//_/_/  - Redistributions in binary form must reproduce the above copyright 
//_/_/    notice, this list of conditions and the following disclaimer in the 
//_/_/    documentation and/or other materials provided with the distribution.
//_/_/
//_/_/  - Neither the name of its copyright holders nor the names of its 
//_/_/    contributors may be used to endorse or promote products derived from 
//_/_/    this software without specific prior written permission.
//_/_/
//_/_/  - CADIA Clause: The license granted in and to the software under this 
//_/_/    agreement is a limited-use license. The software may not be used in 
//_/_/    furtherance of: (i) intentionally causing bodily injury or severe 
//_/_/    emotional harm to any person; (ii) invading the personal privacy or 
//_/_/    violating the human rights of any person; or (iii) committing or 
//_/_/    preparing for any act of war. 
//_/_/
//_/_/  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
//_/_/  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
//_/_/  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
//_/_/  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 
//_/_/  OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
//_/_/  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
//_/_/  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
//_/_/  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
//_/_/  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
//_/_/  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
//_/_/  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//_/_/
//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
/*! @file MathClasses.cpp
    @brief Implementation of the geometry/math value classes declared in MathClasses.h. */
#include "MathClasses.h"
#include "PsyTime.h"
#include "UnitTestFramework.h"
#include <algorithm>
#include <random>

namespace cmlabs {

/////////////////////////////////////////////////////////////////////
// Size Class
/////////////////////////////////////////////////////////////////////

Size::Size() { w=0; h=0; d=0; }
Size::Size(double width, double height, double depth) { w=width; h=height; d=depth; }
Size::~Size() {}

bool Size::equals(const Size &s) const {
	return (
		(this->d == s.d) &&
		(this->w == s.w) &&
		(this->h == s.h) );
}

bool Size::operator==(const Size &s) const {
	return equals(s);
}

Size Size::operator*(const Vector2D &v) const {
	Size s = *this;
	s.w *= v.x;
	s.h *= v.y;
	return s;
}


double Size::getHeight() const { return h; }
double Size::getWidth() const { return w; }
double Size::getDepth() const { return d; }

bool Size::setHeight(double height) { h = height; return true; }
bool Size::setWidth(double width) { w = width; return true; }
bool Size::setDepth(double depth) { d = depth; return true; }

bool Size::isNonZero() const {
	return ( (w != 0) || (h != 0) || (d != 0) );
}

double Size::getArea() const {
	return ( getWidth() * getHeight() );
}

double Size::getDiagonalLenth() const {
	return sqrt( pow(getWidth(), 2) + pow(getHeight(), 2) );
}

std::string Size::print() {
	return utils::StringFormat("(w: %.3f, h: %.3f, d: %.3f)", w, h, d);
}


/////////////////////////////////////////////////////////////////////
// Point Class
/////////////////////////////////////////////////////////////////////

Point::Point() { x=0; y=0; z=0; }
Point::Point(int px, int py, int pz, Size psize) { x=px; y=py; z=pz; size=psize; }
Point::~Point() {}

Point::operator PointFloat() const {
	PointFloat pf;
	pf.setSize(this->size);
	pf.setX(this->x);
	pf.setY(this->y);
	pf.setZ(this->z);
	return pf;
}

int Point::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
	}
	return 0;
}

bool Point::operator==(const Point &p) const {
	return (
		(this->x == p.x) &&
		(this->y == p.y) &&
		(this->z == p.z) &&
		(this->size == p.size) );
}

bool Point::operator==(const PointFloat &p) const {
	return (
		(this->x == (int)p.x) &&
		(this->y == (int)p.y) &&
		(this->z == (int)p.z) &&
		(this->size == p.size) );
}

Point Point::operator-(const Point &p) const {
	Point np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
PointFloat Point::operator-(const PointFloat &p) const {
	PointFloat np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
Point Point::operator+(const Point &p) const {
	Point np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
PointFloat Point::operator+(const PointFloat &p) const {
	PointFloat np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
PointFloat Point::operator+(const Vector2D &v) const {
	PointFloat np = *this;
	np.x += v.x;
	np.y += v.y;
	return np;
}
PointFloat Point::operator+(const Vector3D &v) const {
	PointFloat np = *this;
	np.x += v.x;
	np.y += v.y;
	np.z += v.z;
	return np;
}
Point Point::operator*(const Point &p) const {
	Point np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
PointFloat Point::operator*(const PointFloat &p) const {
	PointFloat np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
Point Point::operator-(double a) const {
	Point np = *this;
	np.x = (int)(np.x - a);
	np.y = (int)(np.y - a);
	np.z = (int)(np.z - a);
	return np;
}
Point Point::operator+(double a) const {
	Point np = *this;
	np.x = (int)(np.x + a);
	np.y = (int)(np.y + a);
	np.z = (int)(np.z + a);
	return np;
}
Point Point::operator*(double a) const {
	Point np = *this;
	np.x = (int)(np.x * a);
	np.y = (int)(np.y * a);
	np.z = (int)(np.z * a);
	return np;
}


int Point::getX() const { return x; }
int Point::getY() const { return y; }
int Point::getZ() const { return z; }
Size Point::getSize() const { return size; }

bool Point::set(int xx, int yy, int zz) { x = xx; y = yy; z = zz; return true; }
bool Point::setX(int n) { x = n; return true; }
bool Point::setY(int n) { y = n; return true; }
bool Point::setZ(int n) { z = n; return true; }
bool Point::setSize(Size s) { size = s; return true; }

double Point::getDistanceTo(Point &p) const {
	return sqrt( pow((double)(getX()-p.getX()), 2) + pow((double)(getY()-p.getY()), 2) );
}
double Point::getDistanceTo(PointFloat &p) const {
	return sqrt( pow((double)(getX()-p.getX()), 2) + pow((double)(getY()-p.getY()), 2) );
}

std::string Point::print() {
	if (size.isNonZero())
		return utils::StringFormat("(%d,%d,%d x %s)", x, y, z, size.print().c_str());
	else
		return utils::StringFormat("(%d,%d,%d)", x, y, z);
}


/////////////////////////////////////////////////////////////////////
// PointFloat Class
/////////////////////////////////////////////////////////////////////

PointFloat::PointFloat() { x=0; y=0; z=0; }
PointFloat::PointFloat(double px, double py, double pz, Size psize) { x=px; y=py; z=pz; size=psize; }
PointFloat::~PointFloat() {}

PointFloat::operator Point() const {
	Point p;
	p.setSize(this->size);
	p.setX((int)this->x);
	p.setY((int)this->y);
	p.setZ((int)this->z);
	return p;
}

double PointFloat::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
	}
	return 0;
}

bool PointFloat::operator==(const Point &p) const {
	return (
		(this->x == p.x) &&
		(this->y == p.y) &&
		(this->z == p.z) &&
		(this->size == p.size) );
}

bool PointFloat::operator==(const PointFloat &p) const {
	return (
		(this->x == p.x) &&
		(this->y == p.y) &&
		(this->z == p.z) &&
		(this->size == p.size) );
}

PointFloat PointFloat::operator-(const Point &p) const {
	PointFloat np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
PointFloat PointFloat::operator-(const PointFloat &p) const {
	PointFloat np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
PointFloat PointFloat::operator+(const Point &p) const {
	PointFloat np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
PointFloat PointFloat::operator+(const PointFloat &p) const {
	PointFloat np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
PointFloat PointFloat::operator+(const Vector2D &v) const {
	PointFloat np = *this;
	np.x += v.x;
	np.y += v.y;
	return np;
}
PointFloat PointFloat::operator+(const Vector3D &v) const {
	PointFloat np = *this;
	np.x += v.x;
	np.y += v.y;
	np.z += v.z;
	return np;
}
PointFloat PointFloat::operator*(const Point &p) const {
	PointFloat np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
PointFloat PointFloat::operator*(const PointFloat &p) const {
	PointFloat np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
PointFloat PointFloat::operator-(double a) const {
	PointFloat np = *this;
	np.x -= a;
	np.y -= a;
	np.z -= a;
	return np;
}
PointFloat PointFloat::operator+(double a) const {
	PointFloat np = *this;
	np.x += a;
	np.y += a;
	np.z += a;
	return np;
}
PointFloat PointFloat::operator*(double a) const {
	PointFloat np = *this;
	np.x *= a;
	np.y *= a;
	np.z *= a;
	return np;
}

double PointFloat::getX() const { return x; }
double PointFloat::getY() const { return y; }
double PointFloat::getZ() const { return z; }
Size PointFloat::getSize() const { return size; }

bool PointFloat::set(double xx, double yy, double zz) { x = xx; y = yy; z = zz; return true; }
bool PointFloat::setX(double n) { x = n; return true; }
bool PointFloat::setY(double n) { y = n; return true; }
bool PointFloat::setZ(double n) { z = n; return true; }
bool PointFloat::setSize(Size s) { size = s; return true; }

double PointFloat::getDistanceTo(const Point &p) const {
	return sqrt( pow((getX()-p.getX()), 2) + pow((getX()-p.getX()), 2) );
}
double PointFloat::getDistanceTo(const PointFloat &p) const {
	return sqrt( pow((getX()-p.getX()), 2) + pow((getX()-p.getX()), 2) );
}

std::string PointFloat::print() {
	if (size.isNonZero()) {
		if (z != 0)
			return utils::StringFormat("(%.3f,%.3f,%.3f x %s)", x, y, z, (char*)size.print().c_str());
		else
			return utils::StringFormat("(%.3f,%.3f x %s)", x, y, (char*)size.print().c_str());
	}
	else {
		if (z != 0)
			return utils::StringFormat("(%.3f,%.3f,%.3f)", x, y, z);
		else
			return utils::StringFormat("(%.3f,%.3f)", x, y);
	}
}





/////////////////////////////////////////////////////////////////////
// Line Class
/////////////////////////////////////////////////////////////////////

Line::Line() {
	lineWidth = 0;
}

Line::Line(PointFloat startpoint, PointFloat endpoint, double width) {
	startPoint = startpoint;
	endPoint = endpoint;
	lineWidth = width;
}

Line::~Line() {
}

PointFloat Line::getStartPoint() const {
	return startPoint;
}
PointFloat Line::getEndPoint() const {
	return endPoint;
}
double Line::getLineWidth() const {
	return lineWidth;
}

bool Line::setStartPoint(PointFloat point) {
	startPoint = point;
	return true;
}
bool Line::setEndPoint(PointFloat point) {
	endPoint = point;
	return true;
}
bool Line::setLineWidth(double width) {
	lineWidth = width;
	return true;
}

std::string Line::print() {
	if (lineWidth != 0)
		return utils::StringFormat("%s-%sx(W: %.3f)", (char*) startPoint.print().c_str(), (char*) endPoint.print().c_str(), lineWidth);
	else
		return utils::StringFormat("%s-%s", (char*) startPoint.print().c_str(), (char*) endPoint.print().c_str());
}



/////////////////////////////////////////////////////////////////////
// PolyLine Class
/////////////////////////////////////////////////////////////////////

PolyLine::PolyLine() {
}

PolyLine::~PolyLine() {}

uint32 PolyLine::getLineCount() const {
	return (uint32)lines.size();
}

Line PolyLine::getLine(uint32 pos) const {
	if (pos > lines.size())
		return Line();
	return lines[pos];
}
bool PolyLine::addLine(Line line) {
	lines.push_back(line);
	return true;
}
bool PolyLine::replaceLine(uint32 pos, Line newline) {
	lines[pos] = newline;
	return true;
}
bool PolyLine::removeLine(uint32 pos) {
	lines[pos] = Line();
	return true;
}

std::string PolyLine::print() {
	if (lines.size() == 0)
		return "PolyLine: (empty)";
	std::string str = "PolyLine: ";
	uint32 n;
	for (n=0; n<=lines.size()-1; n++) {
		str += utils::StringFormat("%s,", (char*)lines[n].print().c_str());
	}
	str += utils::StringFormat("%s", (char*)lines[n].print().c_str());
	return str;
}























/////////////////////////////////////////////////////////////////////
// Box Class
/////////////////////////////////////////////////////////////////////

Box::Box() {
	lineWidth = 0;
	orientation = 0;
}

Box::Box(double x, double y, double w, double h, double linewidth) {
	upperLeft = PointFloat(x, y);
	size = Size(w, h);

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	orientation = 0;
}

Box::Box(PointFloat upperleft, Size boxsize, double linewidth) {
	upperLeft = upperleft;
	size = boxsize;

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	orientation = 0;
}

Box::Box(PointFloat upperleft, PointFloat lowerright, double linewidth) {
	upperLeft = upperleft;
	size = Size(lowerright.getX() - upperleft.getX(), lowerright.getY() - upperleft.getY());

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	orientation = 0;
}

Box::~Box() {
}


double Box::getCMX() const {
	return getCM().getX();
}

double Box::getCMY() const {
	return getCM().getY();
}

PointFloat Box::getCM() const {
	return PointFloat(upperLeft.getX()+(size.getWidth()/2), upperLeft.getY()+(size.getHeight()/2));
}

PointFloat Box::getUpperLeft() const {
	return upperLeft;
}

PointFloat Box::getUpperRight() const {
	return PointFloat(upperLeft.getX()+size.getWidth(), upperLeft.getY());
}

PointFloat Box::getLowerLeft() const {
	return PointFloat(upperLeft.getX(), upperLeft.getY()+size.getHeight());
}

PointFloat Box::getLowerRight() const {
	return PointFloat(upperLeft.getX()+size.getWidth(), upperLeft.getY()+size.getHeight());
}

double Box::getUpperY() const {
	return upperLeft.getY();
}

double Box::getLowerY() const {
	return upperLeft.getY() + size.getHeight();
}

double Box::getLeftX() const {
	return upperLeft.getX();
}

double Box::getRightX() const {
	return upperLeft.getX() + size.getWidth();
}


double Box::getLineWidth() const {
	return lineWidth;
}

Size Box::getSize() const {
	return size;
}

double Box::getArea() const {
	return ( size.w * size.h );
}

double Box::getWidth() const {
	return size.getWidth();
}

double Box::getHeight() const {
	return size.getHeight();
}


Box Box::operator-(const Point &p) const {
	Box newBox = *this;
	newBox.upperLeft = newBox.upperLeft - p;
	return newBox;
}

Box Box::operator-(const PointFloat &p) const {
	Box newBox = *this;
	newBox.upperLeft = newBox.upperLeft - p;
	return newBox;
}

Box Box::operator+(const Point &p) const {
	Box newBox = *this;
	newBox.upperLeft = newBox.upperLeft + p;
	return newBox;
}

Box Box::operator+(const PointFloat &p) const {
	Box newBox = *this;
	newBox.upperLeft = newBox.upperLeft + p;
	return newBox;
}

bool Box::move(double dx, double dy) {
	return set(getLeftX() + dx, getUpperY() + dy, getWidth(), getHeight());
}

bool Box::moveTo(double x, double y) {
	return set(x, y, getWidth(), getHeight());
}

bool Box::grow(double dw, double dh) {
	double w = getWidth() + dw;
	double h = getHeight() + dh;
	return setSize(Size(w, h));
}



Box Box::getConstrainedCopy(const Box& box) {
	Box newbox = *this;
	newbox.constrainTo(box);
	return newbox;
}

Box Box::getConstrainedCopy(double x, double y, double w, double h) {
	Box newbox = *this;
	newbox.constrainTo(x, y, w, h);
	return newbox;
}

bool Box::constrainTo(const Box& box) {
	return constrainTo(box.upperLeft.x, box.upperLeft.y, box.size.w, box.size.h);
}

bool Box::constrainTo(double x, double y, double w, double h) {
	if (upperLeft.x < x)
		upperLeft.x = 0;
	if (upperLeft.y < y)
		upperLeft.y = 0;
	if (upperLeft.x + size.w > w)
		size.w = w - upperLeft.x;
	if (upperLeft.y + size.h > h)
		size.h = h - upperLeft.y;
	return true;
}

Box Box::getDoubleSizeSameCenter() {
	Box box;
	box.upperLeft.x = upperLeft.x - (size.w / 2.0);
	box.upperLeft.y = upperLeft.y - (size.h / 2.0);
	box.size.w = size.w * 2.0;
	box.size.h = size.h * 2.0;
	return box;
}

bool Box::set(double x, double y, double w, double h, double linewidth) {
	upperLeft = PointFloat(x, y);
	size = Size(w, h);

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	return true;
}


bool Box::setUpperLeft(const PointFloat& point) {
	upperLeft = point;
	return true;
}

bool Box::setSize(const Size& boxsize) {
	size = boxsize;
	return true;
}

bool Box::setLineWidth(double width) {
	lineWidth = width;
	return true;
}


bool Box::isPointWithin(int x, int y) const {
	if ( x > this->getRightX() )
		return false;
	else if ( x < this->getLeftX() )
		return false;
	else if ( y < this->getUpperY() )
		return false;
	else if ( y > this->getLowerY() )
		return false;
	return true;
}

bool Box::isPointWithin(const PointFloat& point) const {
	if ( point.getX() > this->getRightX() )
		return false;
	else if ( point.getX() < this->getLeftX() )
		return false;
	else if ( point.getY() < this->getUpperY() )
		return false;
	else if ( point.getY() > this->getLowerY() )
		return false;
	return true;
}

PointFloat Box::getCentreMass() const {
	return PointFloat(upperLeft.getX()+(size.getWidth()/2), upperLeft.getY()+(size.getHeight()/2));
}

bool Box::hasZeroSize() const {
	return (!size.isNonZero());
}

bool Box::equals(const Box &otherbox) const {
	return (
		(this->size == otherbox.size) &&
		(this->upperLeft == otherbox.upperLeft) &&
		(this->orientation == otherbox.orientation) );
}

bool Box::equals(const Box &otherbox, double maxerror) const {
	if (maxerror == 0.0)
		maxerror = -0.001;
	return (percentOverlap(otherbox) <= maxerror);
}

bool Box::growToBoundingBox(const Box &otherbox) {
	Box bb = getBoundingBox(otherbox);
	if (bb.hasZeroSize())
		return false;
	this->upperLeft = bb.upperLeft;
	this->size = bb.size;
	return true;
}

bool Box::growToIncludePoint(const Point& point, int padX, int padY) {
	double dif;
	if ( ( dif = upperLeft.x - (point.x - padX) ) > 0 ) {
		upperLeft.x -= dif;
		size.w += dif;
	}
	if ( ( dif = upperLeft.y - (point.y - padY) ) > 0 ) {
		upperLeft.y -= dif;
		size.h += dif;
	}

	if ( ( dif = point.x + padX - (upperLeft.x + size.w) ) > 0 ) {
		size.w += dif;
	}
	if ( ( dif = point.y + padY - (upperLeft.y + size.h) ) > 0 ) {
		size.h += dif;
	}
	return true;
}

Box Box::getBoundingBox(const Box &otherbox) const {

	double left, right, top, bottom;

	if (this->getLeftX() <= otherbox.getLeftX())
		left = this->getLeftX();
	else
		left = otherbox.getLeftX();

	if (this->getRightX() <= otherbox.getRightX())
		right = otherbox.getRightX();
	else
		right = this->getRightX();

	if (this->getUpperY() <= otherbox.getUpperY())
		top = this->getUpperY();
	else
		top = otherbox.getUpperY();

	if (this->getLowerY() <= otherbox.getLowerY())
		bottom = otherbox.getLowerY();
	else
		bottom = this->getLowerY();

	return Box(PointFloat(left, top), PointFloat(right, bottom));
}

Box Box::getOverlapBox(const Box &otherbox) const {

	Box zeroOverlap = Box();

	// Rule out zero overlap first
	if ( otherbox.getLeftX() > this->getRightX() )
		return zeroOverlap;
	else if ( otherbox.getRightX() < this->getLeftX() )
		return zeroOverlap;
	else if ( otherbox.getLowerY() < this->getUpperY() )
		return zeroOverlap;
	else if ( otherbox.getUpperY() > this->getLowerY() )
		return zeroOverlap;

	// Now we know there is an overlap

	double left, right, top, bottom;

	if (this->getLeftX() <= otherbox.getLeftX())
		left = otherbox.getLeftX();
	else
		left = this->getLeftX();

	if (this->getRightX() <= otherbox.getRightX())
		right = this->getRightX();
	else
		right = otherbox.getRightX();

	if (this->getUpperY() <= otherbox.getUpperY())
		top = otherbox.getUpperY();
	else
		top = this->getUpperY();

	if (this->getLowerY() <= otherbox.getLowerY())
		bottom = this->getLowerY();
	else
		bottom = otherbox.getLowerY();

	return Box(PointFloat(left, top), PointFloat(right, bottom));
}

double Box::percentOverlap(const Box &otherbox) const {

	Box overlap = getOverlapBox(otherbox);
	if (!overlap.getSize().isNonZero())
		return 0.0;

	double myArea = size.getArea();
	double overlapArea = overlap.getSize().getArea();
	
	
	if ( (myArea > overlapArea) && (myArea != 0.0) )
		return ( overlapArea / myArea );
	else if (overlapArea != 0.0)
		return ( myArea / overlapArea );
	else
		return 0;
}






std::string Box::print() {
	if (lineWidth != 0)
		return utils::StringFormat("%s-%sx(W: %.3f)", (char*) getUpperLeft().print().c_str(), (char*) getLowerRight().print().c_str(), lineWidth);
	else
		return utils::StringFormat("%s-%s", (char*) getUpperLeft().print().c_str(), (char*) getLowerRight().print().c_str());
}



















/////////////////////////////////////////////////////////////////////
// Vector2D Class
/////////////////////////////////////////////////////////////////////

Vector2D::Vector2D() { x=0; y=0; }
Vector2D::Vector2D(double x, double y) { this->x=x; this->y=y; }
Vector2D::~Vector2D() {}

double Vector2D::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
	}
	return 0;
}

bool Vector2D::operator==(const Vector2D &v) const {
	return (
		(this->x == v.x) &&
		(this->y == v.y) );
}

Vector2D Vector2D::operator-(const Vector2D &v) const {
	Vector2D np = *this;
	np.x -= v.x;
	np.y -= v.y;
	return np;
}

Vector2D Vector2D::operator+(const Vector2D &v) const {
	Vector2D np = *this;
	np.x += v.x;
	np.y += v.y;
	return np;
}

double Vector2D::operator*(const Vector2D &v) const {
	return (x*v.x) + (y*v.y);
}

Vector2D Vector2D::operator-(double a) const {
	Vector2D np = *this;
	np.x -= a;
	np.y -= a;
	return np;
}

Vector2D Vector2D::operator+(double a) const {
	Vector2D np = *this;
	np.x += a;
	np.y += a;
	return np;
}

Vector2D Vector2D::operator*(double a) const {
	Vector2D np = *this;
	np.x *= a;
	np.y *= a;
	return np;
}

const Vector2D& Vector2D::operator-=(const Vector2D &v) {
	this->x -= v.x;
	this->y -= v.y;
	return *this;
}

const Vector2D& Vector2D::operator+=(const Vector2D &v) {
	this->x += v.x;
	this->y += v.y;
	return *this;
}

const Vector2D& Vector2D::operator-=(double a) {
	this->x -= a;
	this->y -= a;
	return *this;
}

const Vector2D& Vector2D::operator+=(double a) {
	this->x += a;
	this->y += a;
	return *this;
}

const Vector2D& Vector2D::operator*=(double a) {
	this->x *= a;
	this->y *= a;
	return *this;
}



double Vector2D::getX() const { return x; }
double Vector2D::getY() const { return y; }

bool Vector2D::set(const PointFloat& p1, const PointFloat& p2) {
	return set(p1.x, p1.y, p2.x, p2.y);
}

bool Vector2D::set(double x1, double y1, double x2, double y2) {
	return set(x2-x1, y2-y1);
}

bool Vector2D::set(double xx, double yy) { x = xx; y = yy; return true; }
bool Vector2D::setX(double n) { x = n; return true; }
bool Vector2D::setY(double n) { y = n; return true; }
bool Vector2D::setLength(double l) {
	double len = length();
	if (len == 0) return false;
	double v = l / len;
	x *= v;
	y *= v;
	return true;
}

std::string Vector2D::print() {
	return utils::StringFormat("(%.3f,%.3f)", x, y);
}

double Vector2D::length() const {
	return sqrt((x*x) + (y*y));
}

double Vector2D::det(const Vector2D &v) const {
	return (x*v.y) - (y*v.x);
}

bool Vector2D::isOrthogonalWith(const Vector2D &v) const {
	return (*this*v == 0);
}

bool Vector2D::isParallelWith(const Vector2D &v) const {
	return (det(v) == 0);
}

Vector2D Vector2D::getProjectionOn(const Vector2D &v) const {
	Vector2D unit = v.getUnitVector();
	return unit*(*this*unit);
}

bool Vector2D::makeUnitVector() {
	return setLength(1.0);
}

Vector2D Vector2D::getUnitVector() const {
	Vector2D vect;
	double len = length();
	if (len == 0) return vect;
	double v = 1.0 / len;
	vect.x = x*v;
	vect.y = y*v;
	return vect;
}

Vector2D Vector2D::rotate(double angle) {
	Vector2D vect;
	if ((x == 0) && (y == 0)) return vect;
	vect.x = x * cos(angle) - y * sin(angle);
	vect.y = x * sin(angle) + y * cos(angle);
	return vect;
}

bool Vector2D::rotateIt(double angle) {
	if ((x == 0) && (y == 0)) return false;
	double newX = x * cos(angle) - y * sin(angle);
	double newY = x * sin(angle) + y * cos(angle);
	x = newX;
	y = newY;
	return true;
}

Vector2D Vector2D::getOrthogonalVector() const {
	return Vector2D(0-y, x);
}

double Vector2D::getAngle(const Vector2D &v) const {
	double lengths = this->length() * v.length();
	if (lengths == 0)
		return 0;
	return acos((*this*v)/lengths);
}

double Vector2D::getArea(const Vector2D &v) const {
	return fabs(det(v));
}




/////////////////////////////////////////////////////////////////////
// Vector3D Class
/////////////////////////////////////////////////////////////////////

Vector3D::Vector3D() { x=0; y=0; z=0; }
Vector3D::Vector3D(double x, double y, double z) { this->x=x; this->y=y; this->z=z; }
Vector3D::~Vector3D() {}

double Vector3D::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
	}
	return 0;
}

bool Vector3D::operator==(const Vector3D &v) const {
	return (
		(this->x == v.x) &&
		(this->y == v.y) &&
		(this->z == v.z) );
}

Vector3D Vector3D::operator-(const Vector3D &v) const {
	Vector3D np = *this;
	np.x -= v.x;
	np.y -= v.y;
	np.z -= v.z;
	return np;
}

Vector3D Vector3D::operator+(const Vector3D &v) const {
	Vector3D np = *this;
	np.x += v.x;
	np.y += v.y;
	np.z += v.z;
	return np;
}

double Vector3D::operator*(const Vector3D &v) const {
	return (x*v.x) + (y*v.y) + (z*v.z);
}

Vector3D Vector3D::operator-(double a) const {
	Vector3D np = *this;
	np.x -= a;
	np.y -= a;
	np.z -= a;
	return np;
}

Vector3D Vector3D::operator+(double a) const {
	Vector3D np = *this;
	np.x += a;
	np.y += a;
	np.z += a;
	return np;
}

Vector3D Vector3D::operator*(double a) const {
	Vector3D np = *this;
	np.x *= a;
	np.y *= a;
	np.z *= a;
	return np;
}

const Vector3D& Vector3D::operator-=(const Vector3D &v) {
	this->x -= v.x;
	this->y -= v.y;
	this->z -= v.z;
	return *this;
}

const Vector3D& Vector3D::operator+=(const Vector3D &v) {
	this->x += v.x;
	this->y += v.y;
	this->z += v.z;
	return *this;
}

const Vector3D& Vector3D::operator-=(double a) {
	this->x -= a;
	this->y -= a;
	this->z -= a;
	return *this;
}

const Vector3D& Vector3D::operator+=(double a) {
	this->x += a;
	this->y += a;
	this->z += a;
	return *this;
}

const Vector3D& Vector3D::operator*=(double a) {
	this->x *= a;
	this->y *= a;
	this->z *= a;
	return *this;
}



double Vector3D::getX() const { return x; }
double Vector3D::getY() const { return y; }
double Vector3D::getZ() const { return z; }

bool Vector3D::set(const PointFloat& p1, const PointFloat& p2) {
	return set(p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
}

bool Vector3D::set(double x1, double y1, double z1, double x2, double y2, double z2) {
	return set(x2-x1, y2-y1, z2-z1);
}

bool Vector3D::set(double xx, double yy, double zz) { x = xx; y = yy; z = zz; return true; }
bool Vector3D::setX(double n) { x = n; return true; }
bool Vector3D::setY(double n) { y = n; return true; }
bool Vector3D::setZ(double n) { z = n; return true; }

std::string Vector3D::print() {
	return utils::StringFormat("(%.3f,%.3f,%.3f)", x, y, z);
}

double Vector3D::length() const {
	return sqrt((x*x) + (y*y) + (z*z));
}

Vector3D Vector3D::product(const Vector3D &v) const {
	return Vector3D(
		this->y*v.z - this->z*v.y,
		this->z*v.x - this->x*v.z,
		this->x*v.y - this->y*v.x);
}

bool Vector3D::isOrthogonalWith(const Vector3D &v) const {
	return (*this*v == 0);
}

bool Vector3D::isParallelWith(const Vector3D &v) const {
	return (product(v).length() == 0);
}

Vector3D Vector3D::getProjectionOn(const Vector3D &v) const {
	Vector3D unit = v.getUnitVector();
	return unit*(*this*unit);
}

Vector3D Vector3D::getUnitVector() const {
	return *this*(1.0/length());
}

double Vector3D::getAngle(const Vector3D &v) const {
	double lengths = this->length() * v.length();
	if (lengths == 0)
		return 0;
	return acos((*this*v)/lengths);
}

double Vector3D::getArea(const Vector3D &v) const {
	return fabs(product(v).length());
}







Color::Color() {
	init(0, 0, 0);
}

Color::Color(uint8 red, uint8 green, uint8 blue) {
	init(red, green, blue);
}

Color::Color(const Color &color) {
	init(color.r, color.g, color.b);
	size = color.size;
}

Color::Color(const char* colorName) {
	init(colorName);
}

Color::~Color() {
}

bool Color::init(uint8 red, uint8 green, uint8 blue) {
	r = red;
	g = green;
	b = blue;
	size = 1;
	return true;
}

bool Color::init(const char* colorName) {
	
	if (!colorName || !strlen(colorName))
		return init(0, 0, 0);

	if (strchr(colorName, ',')) {
		std::vector<std::string> col = utils::TextListSplit(colorName, ",", true);
		if (col.size() < 3)
			return false;
		else if (col.size() == 3)
			return init(
				(uint8)utils::Ascii2Uint32(col[0].c_str()),
				(uint8)utils::Ascii2Uint32(col[1].c_str()),
				(uint8)utils::Ascii2Uint32(col[2].c_str())
			);
		else {
			size = utils::Ascii2Uint32(col[3].c_str());
			return init(
				(uint8)utils::Ascii2Uint32(col[0].c_str()),
				(uint8)utils::Ascii2Uint32(col[1].c_str()),
				(uint8)utils::Ascii2Uint32(col[2].c_str())
				);
		}
	}
	
	std::map<std::string, std::array<uint8, 3> >::const_iterator i = ColorMap.find(colorName);
	if (i != ColorMap.end())
		return init((*i).second.at(0), (*i).second.at(1), (*i).second.at(2));
	else
		return init(0, 0, 0);
}

//bool Color::setNoColor() {
//	r = g = b = -1;
//	return true;
//}
//
//bool Color::isValid() {
//	return (!isNoColor());
//}
//
//bool Color::isNoColor() {
//	return ((r < 0) || (g < 0) || (b < 0));
//}

Color Color::mixOn(Color& color, double weight) {
	// color is the background color
	if (weight > 1)
		weight = 1;
	if (weight < 0)
		weight = 0;
	Color resColor;
	resColor.r = (uint8)(color.r*(1 - weight) + this->r*weight);
	resColor.g = (uint8)(color.g*(1 - weight) + this->g*weight);
	resColor.b = (uint8)(color.b*(1 - weight) + this->b*weight);
	return resColor;
}

uint8 Color::getGreyValue() {
	return (uint8)(((double)r + g + b) / 3.0);
}

Color Color::getReverseColor() {
	Color resColor;
	resColor.r = 255 - r;
	resColor.g = 255 - g;
	resColor.b = 255 - b;
	return resColor;
}

double Color::distance(Color &otherColor) {
	return pow((double)pow((double)r - otherColor.r, 2) + pow((double)g - otherColor.g, 2) + pow((double)b - otherColor.b, 2), (1.0 / 3));
}

std::vector<Color> Color::createColorsMaxDifference(uint32 count, bool mix) {

	int rcount = (int)(count / 3);
	if (rcount <= 0) rcount = 1;
	int gcount = (int)(count / 3);
	if (gcount <= 0) gcount = 1;
	int bcount = count - rcount - gcount;
	if (bcount <= 0) bcount = 1;

	int n;
	std::vector<Color> col;
	int value = 0;
	for (n = 0; n<rcount; n++) {
		value = (int)((n + 1)*(255.0 / rcount));
		if (value >= 256) value = 255;
		col.push_back(Color(value, 0, 0));
	}
	for (n = 0; n<gcount; n++) {
		value = (int)((n + 1)*(255.0 / gcount));
		if (value >= 256) value = 255;
		col.push_back(Color(0, value, 0));
	}
	for (n = 0; n<bcount; n++) {
		value = (int)((n + 1)*(255.0 / bcount));
		if (value >= 256) value = 255;
		col.push_back(Color(0, 0, value));
	}

	if (mix) {
		// std::random_shuffle(col.begin(), col.end());
		std::random_device rd;
		std::mt19937 g(rd());
		std::shuffle(col.begin(), col.end(), g);
	}
	return col;
}




/////////////////////////////////////////////////////////////////////
// RandomPathGenerator Class
/////////////////////////////////////////////////////////////////////

RandomPathGenerator::RandomPathGenerator() {
	lastPointTime = 0;
	srand((uint32)(uint64)this);
}

RandomPathGenerator::~RandomPathGenerator() {
}

bool RandomPathGenerator::setStartPoint(double x, double y) {
	lastPoint.x = x;
	lastPoint.y = y;
	lastPointTime = GetTimeNow();
	return true;
}

bool RandomPathGenerator::setDirection(double dx, double dy) {
	direction.x = dx;
	direction.y = dy;
	return true;
}

bool RandomPathGenerator::setVariation(double var) {
	variation.x = variation.y = var;
	return true;
}

PointFloat RandomPathGenerator::generateNextPoint() {
	double secs = (double)GetTimeAge(lastPointTime) / 1000000.0;
	//Interpolate next exact point from last point and timestamp
	PointFloat nextPoint, dynPoint;
	nextPoint.x = lastPoint.x + (secs * direction.x);
	nextPoint.y = lastPoint.y + (secs * direction.y);
	//Move exact point by random variation
	dynPoint.x = utils::RandomValue(nextPoint.x - variation.x, nextPoint.x + variation.x);
	dynPoint.y = utils::RandomValue(nextPoint.y - variation.y, nextPoint.y + variation.y);
	//Publish point
	lastPoint = nextPoint;
	lastPointTime = GetTimeNow();
	return dynPoint;
}


// ################# Unit Test #################

bool MathClasses_UnitTest() {
	const double EPS = 1e-9;

	// ---- Size ----
	unittest::progress(5, "Size");
	{
		Size s(3.0, 4.0);
		if (fabs(s.getArea() - 12.0) > EPS) { unittest::fail("MathClasses test: Size::getArea expected 12 got %.6f\n", s.getArea()); return false; }
		// diagonal of 3,4 right triangle = 5
		if (fabs(s.getDiagonalLenth() - 5.0) > EPS) { unittest::fail("MathClasses test: Size::getDiagonalLenth expected 5 got %.6f\n", s.getDiagonalLenth()); return false; }
		if (!s.isNonZero()) { unittest::fail("MathClasses test: Size::isNonZero expected true\n"); return false; }
		Size zero;
		if (zero.isNonZero()) { unittest::fail("MathClasses test: empty Size::isNonZero expected false\n"); return false; }
		Size s2(3.0, 4.0);
		if (!(s == s2)) { unittest::fail("MathClasses test: Size::operator== expected equal\n"); return false; }
		// Size scaled by a Vector2D multiplies w by v.x and h by v.y
		Size scaled = s * Vector2D(2.0, 0.5);
		if (fabs(scaled.getWidth() - 6.0) > EPS || fabs(scaled.getHeight() - 2.0) > EPS) {
			unittest::fail("MathClasses test: Size::operator*(Vector2D) expected (6,2) got (%.3f,%.3f)\n", scaled.getWidth(), scaled.getHeight()); return false;
		}
	}

	// ---- Point ----
	unittest::progress(15, "Point");
	{
		Point a(0, 0);
		Point b(3, 4);
		if (fabs(a.getDistanceTo(b) - 5.0) > EPS) { unittest::fail("MathClasses test: Point::getDistanceTo expected 5 got %.6f\n", a.getDistanceTo(b)); return false; }
		Point diff = b - a;
		if (diff.getX() != 3 || diff.getY() != 4) { unittest::fail("MathClasses test: Point::operator- expected (3,4) got (%d,%d)\n", diff.getX(), diff.getY()); return false; }
		Point sum = a + b;
		if (sum.getX() != 3 || sum.getY() != 4) { unittest::fail("MathClasses test: Point::operator+ expected (3,4)\n"); return false; }
		if (!(b == Point(3, 4))) { unittest::fail("MathClasses test: Point::operator== expected equal\n"); return false; }
	}

	// ---- Vector2D ----
	unittest::progress(30, "Vector2D");
	{
		Vector2D v(3.0, 4.0);
		if (fabs(v.length() - 5.0) > EPS) { unittest::fail("MathClasses test: Vector2D::length expected 5 got %.6f\n", v.length()); return false; }

		// dot product
		Vector2D a(1.0, 2.0), b(3.0, 4.0);
		double dot = a * b;           // 1*3 + 2*4 = 11
		if (fabs(dot - 11.0) > EPS) { unittest::fail("MathClasses test: Vector2D dot expected 11 got %.6f\n", dot); return false; }

		// determinant: 1*4 - 2*3 = -2
		if (fabs(a.det(b) - (-2.0)) > EPS) { unittest::fail("MathClasses test: Vector2D::det expected -2 got %.6f\n", a.det(b)); return false; }

		// area = |det| = 2
		if (fabs(a.getArea(b) - 2.0) > EPS) { unittest::fail("MathClasses test: Vector2D::getArea expected 2 got %.6f\n", a.getArea(b)); return false; }

		// orthogonality: (1,0) and (0,1)
		Vector2D ex(1.0, 0.0), ey(0.0, 1.0);
		if (!ex.isOrthogonalWith(ey)) { unittest::fail("MathClasses test: Vector2D::isOrthogonalWith expected true\n"); return false; }
		if (ex.isOrthogonalWith(ex)) { unittest::fail("MathClasses test: Vector2D::isOrthogonalWith(self) expected false\n"); return false; }

		// parallel: (1,1) and (2,2)
		if (!Vector2D(1.0, 1.0).isParallelWith(Vector2D(2.0, 2.0))) { unittest::fail("MathClasses test: Vector2D::isParallelWith expected true\n"); return false; }

		// angle between ex and ey = pi/2
		if (fabs(ex.getAngle(ey) - (M_PI / 2.0)) > 1e-6) { unittest::fail("MathClasses test: Vector2D::getAngle expected pi/2 got %.6f\n", ex.getAngle(ey)); return false; }

		// unit vector of (3,4) has length 1, direction preserved
		Vector2D unit = v.getUnitVector();
		if (fabs(unit.length() - 1.0) > EPS) { unittest::fail("MathClasses test: Vector2D::getUnitVector length expected 1 got %.6f\n", unit.length()); return false; }
		if (fabs(unit.x - 0.6) > EPS || fabs(unit.y - 0.8) > EPS) { unittest::fail("MathClasses test: Vector2D::getUnitVector expected (0.6,0.8) got (%.3f,%.3f)\n", unit.x, unit.y); return false; }

		// orthogonal vector of (1,0) is (0,1)
		Vector2D ortho = ex.getOrthogonalVector();
		if (fabs(ortho.x - 0.0) > EPS || fabs(ortho.y - 1.0) > EPS) { unittest::fail("MathClasses test: Vector2D::getOrthogonalVector expected (0,1) got (%.3f,%.3f)\n", ortho.x, ortho.y); return false; }
		if (!ex.isOrthogonalWith(ortho)) { unittest::fail("MathClasses test: orthogonal vector not orthogonal\n"); return false; }

		// projection of (2,3) on x-axis is (2,0)
		Vector2D proj = Vector2D(2.0, 3.0).getProjectionOn(ex);
		if (fabs(proj.x - 2.0) > EPS || fabs(proj.y - 0.0) > EPS) { unittest::fail("MathClasses test: Vector2D::getProjectionOn expected (2,0) got (%.3f,%.3f)\n", proj.x, proj.y); return false; }

		// rotate (1,0) by 90 degrees -> (0,1)
		Vector2D rot = ex.rotate(M_PI / 2.0);
		if (fabs(rot.x - 0.0) > 1e-9 || fabs(rot.y - 1.0) > 1e-9) { unittest::fail("MathClasses test: Vector2D::rotate(pi/2) expected (0,1) got (%.6f,%.6f)\n", rot.x, rot.y); return false; }

		// setLength scales to requested length, makeUnitVector -> length 1
		Vector2D sl(3.0, 4.0);
		if (!sl.setLength(10.0)) { unittest::fail("MathClasses test: Vector2D::setLength returned false\n"); return false; }
		if (fabs(sl.length() - 10.0) > 1e-6) { unittest::fail("MathClasses test: Vector2D::setLength expected length 10 got %.6f\n", sl.length()); return false; }
		Vector2D mu(5.0, 0.0);
		mu.makeUnitVector();
		if (fabs(mu.length() - 1.0) > EPS) { unittest::fail("MathClasses test: Vector2D::makeUnitVector expected length 1 got %.6f\n", mu.length()); return false; }

		// compound assignment & arithmetic operators
		Vector2D c(1.0, 1.0);
		c += Vector2D(2.0, 3.0);     // (3,4)
		if (fabs(c.x - 3.0) > EPS || fabs(c.y - 4.0) > EPS) { unittest::fail("MathClasses test: Vector2D::operator+= expected (3,4)\n"); return false; }
		c *= 2.0;                    // (6,8)
		if (fabs(c.x - 6.0) > EPS || fabs(c.y - 8.0) > EPS) { unittest::fail("MathClasses test: Vector2D::operator*= expected (6,8)\n"); return false; }
		Vector2D scaledV = Vector2D(2.0, 3.0) * 2.0;
		if (!(scaledV == Vector2D(4.0, 6.0))) { unittest::fail("MathClasses test: Vector2D::operator*(double) expected (4,6)\n"); return false; }
	}

	// ---- Vector3D ----
	unittest::progress(50, "Vector3D");
	{
		// length of (2,3,6) = 7
		Vector3D v(2.0, 3.0, 6.0);
		if (fabs(v.length() - 7.0) > EPS) { unittest::fail("MathClasses test: Vector3D::length expected 7 got %.6f\n", v.length()); return false; }

		// dot product (1,2,3).(4,5,6) = 32
		Vector3D a(1.0, 2.0, 3.0), b(4.0, 5.0, 6.0);
		double dot = a * b;
		if (fabs(dot - 32.0) > EPS) { unittest::fail("MathClasses test: Vector3D dot expected 32 got %.6f\n", dot); return false; }

		// cross product x cross y = z
		Vector3D ex(1.0, 0.0, 0.0), ey(0.0, 1.0, 0.0), ez(0.0, 0.0, 1.0);
		Vector3D cross = ex.product(ey);
		if (!(cross == ez)) { unittest::fail("MathClasses test: Vector3D::product(x,y) expected z got (%.3f,%.3f,%.3f)\n", cross.x, cross.y, cross.z); return false; }

		// orthogonality of axes
		if (!ex.isOrthogonalWith(ey)) { unittest::fail("MathClasses test: Vector3D::isOrthogonalWith expected true\n"); return false; }
		if (ex.isOrthogonalWith(ex)) { unittest::fail("MathClasses test: Vector3D::isOrthogonalWith(self) expected false\n"); return false; }

		// parallel
		if (!Vector3D(1.0, 2.0, 3.0).isParallelWith(Vector3D(2.0, 4.0, 6.0))) { unittest::fail("MathClasses test: Vector3D::isParallelWith expected true\n"); return false; }

		// angle between x and y = pi/2
		if (fabs(ex.getAngle(ey) - (M_PI / 2.0)) > 1e-6) { unittest::fail("MathClasses test: Vector3D::getAngle expected pi/2 got %.6f\n", ex.getAngle(ey)); return false; }

		// unit vector length 1
		Vector3D unit = v.getUnitVector();
		if (fabs(unit.length() - 1.0) > EPS) { unittest::fail("MathClasses test: Vector3D::getUnitVector length expected 1 got %.6f\n", unit.length()); return false; }

		// area = |cross| ; for unit x,y axes that is 1
		if (fabs(ex.getArea(ey) - 1.0) > EPS) { unittest::fail("MathClasses test: Vector3D::getArea expected 1 got %.6f\n", ex.getArea(ey)); return false; }

		// projection of (2,3,5) on x-axis is (2,0,0)
		Vector3D proj = Vector3D(2.0, 3.0, 5.0).getProjectionOn(ex);
		if (fabs(proj.x - 2.0) > EPS || fabs(proj.y) > EPS || fabs(proj.z) > EPS) { unittest::fail("MathClasses test: Vector3D::getProjectionOn expected (2,0,0)\n"); return false; }
	}

	// ---- Box ----
	unittest::progress(70, "Box");
	{
		Box box(0.0, 0.0, 10.0, 20.0);
		if (fabs(box.getArea() - 200.0) > EPS) { unittest::fail("MathClasses test: Box::getArea expected 200 got %.6f\n", box.getArea()); return false; }
		if (fabs(box.getRightX() - 10.0) > EPS || fabs(box.getLowerY() - 20.0) > EPS) { unittest::fail("MathClasses test: Box edges wrong\n"); return false; }

		// centre of mass at (5,10)
		PointFloat cm = box.getCM();
		if (fabs(cm.getX() - 5.0) > EPS || fabs(cm.getY() - 10.0) > EPS) { unittest::fail("MathClasses test: Box::getCM expected (5,10) got (%.3f,%.3f)\n", cm.getX(), cm.getY()); return false; }

		// point containment
		if (!box.isPointWithin(PointFloat(5.0, 5.0))) { unittest::fail("MathClasses test: Box::isPointWithin inside expected true\n"); return false; }
		if (box.isPointWithin(PointFloat(15.0, 5.0))) { unittest::fail("MathClasses test: Box::isPointWithin outside expected false\n"); return false; }

		// negative size box gets normalised so upperLeft is the true upper-left
		Box neg(10.0, 10.0, -5.0, -5.0);
		if (fabs(neg.getLeftX() - 5.0) > EPS || fabs(neg.getUpperY() - 5.0) > EPS) { unittest::fail("MathClasses test: Box negative-size normalisation wrong (%.3f,%.3f)\n", neg.getLeftX(), neg.getUpperY()); return false; }
		if (fabs(neg.getWidth() - 5.0) > EPS || fabs(neg.getHeight() - 5.0) > EPS) { unittest::fail("MathClasses test: Box negative-size dimensions wrong\n"); return false; }

		// overlap of two 10x10 boxes offset by 5 -> 5x5 = 25 area
		Box b1(0.0, 0.0, 10.0, 10.0);
		Box b2(5.0, 5.0, 10.0, 10.0);
		Box overlap = b1.getOverlapBox(b2);
		if (fabs(overlap.getArea() - 25.0) > EPS) { unittest::fail("MathClasses test: Box::getOverlapBox area expected 25 got %.6f\n", overlap.getArea()); return false; }

		// bounding box of the two should be 15x15 = 225 area, upper-left at (0,0)
		Box bb = b1.getBoundingBox(b2);
		if (fabs(bb.getLeftX()) > EPS || fabs(bb.getUpperY()) > EPS) { unittest::fail("MathClasses test: Box::getBoundingBox origin expected (0,0)\n"); return false; }
		if (fabs(bb.getArea() - 225.0) > EPS) { unittest::fail("MathClasses test: Box::getBoundingBox area expected 225 got %.6f\n", bb.getArea()); return false; }

		// percentOverlap: overlap 25 / myArea 100 = 0.25
		double pct = b1.percentOverlap(b2);
		if (fabs(pct - 0.25) > EPS) { unittest::fail("MathClasses test: Box::percentOverlap expected 0.25 got %.6f\n", pct); return false; }

		// disjoint boxes => zero overlap area
		Box f1(100.0, 100.0, 10.0, 10.0);
		Box noOverlap = b1.getOverlapBox(f1);
		if (noOverlap.getSize().isNonZero()) { unittest::fail("MathClasses test: disjoint Box::getOverlapBox expected zero size\n"); return false; }
		if (fabs(b1.percentOverlap(f1)) > EPS) { unittest::fail("MathClasses test: disjoint percentOverlap expected 0\n"); return false; }

		// move / grow
		Box mv(0.0, 0.0, 4.0, 4.0);
		mv.move(2.0, 3.0);
		if (fabs(mv.getLeftX() - 2.0) > EPS || fabs(mv.getUpperY() - 3.0) > EPS) { unittest::fail("MathClasses test: Box::move wrong\n"); return false; }
		mv.grow(2.0, 0.0);
		if (fabs(mv.getWidth() - 6.0) > EPS) { unittest::fail("MathClasses test: Box::grow width expected 6 got %.6f\n", mv.getWidth()); return false; }
	}

	// ---- Throughput metric ----
	unittest::progress(85, "throughput");
	{
		const int N = 200000;
		Vector2D acc(1.0, 1.0);
		double sink = 0.0;
		uint64 t0 = GetTimeNow();
		for (int i = 0; i < N; i++) {
			Vector2D a((double)(i % 17), (double)(i % 13));
			Vector2D b((double)(i % 7) + 1.0, (double)(i % 5) + 1.0);
			sink += a * b;                  // dot
			sink += a.det(b);               // det
			acc = acc + a.getUnitVector();  // unit + add
		}
		double us = (double)(GetTimeNow() - t0);
		unittest::detail("Vector2D ops: N=%d, sink=%.3f, acc=(%.3f,%.3f)\n", N, sink, acc.x, acc.y);
		if (us > 0.0)
			unittest::metric("vector2d_op_throughput", (double)N / us * 1e6, "ops/s", true);
	}

	unittest::progress(100, "done");
	return true;
}

void Register_MathClasses_Tests() {
	UnitTestRunner::instance().registerTest("math", MathClasses_UnitTest,
		"Geometry math: Size, Point, Vector2D, Vector3D, Box", "math");
}

} // namespace cmlabs
