You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
595 lines
11 KiB
C++
595 lines
11 KiB
C++
#include "MLVector.h"
|
|
//#include "MLLine.h"
|
|
|
|
const MLVector MLVector::invalid = MLVector(0, 0, 0, false);
|
|
const MLVector MLVector::nullVector = MLVector(0, 0, 0, true);
|
|
|
|
MLVector::MLVector(void)
|
|
:m_x(0.0), m_y(0.0), m_z(0.0), m_valid(true)
|
|
{
|
|
|
|
}
|
|
|
|
MLVector::MLVector( double vx, double vy, double vz /*= 0.0*/, bool valid_in /*= true*/ )
|
|
: m_x(vx), m_y(vy), m_z(vz)
|
|
{
|
|
valid = valid_in;
|
|
}
|
|
|
|
|
|
MLVector::~MLVector(void)
|
|
{
|
|
}
|
|
|
|
void MLVector::set(double vx, double vy, double vz)
|
|
{
|
|
x = vx;
|
|
y = vy;
|
|
z = vz;
|
|
valid = true;
|
|
}
|
|
|
|
/**
|
|
* Sets a new position for the vector in polar coordinates.
|
|
*
|
|
* \param radius the radius or the distance
|
|
*
|
|
* \param angle the angle in rad
|
|
*/
|
|
void MLVector::setPolar(double radius, double angle)
|
|
{
|
|
x = radius * cos(angle);
|
|
y = radius * sin(angle);
|
|
z = 0.0;
|
|
//valid = MLMath::isNormal(radius) && MLMath::isNormal(angle);
|
|
}
|
|
|
|
bool MLVector::isValid() const
|
|
{
|
|
return valid;
|
|
}
|
|
|
|
bool MLVector::isInside( const MLBox& b ) const
|
|
{
|
|
/*
|
|
MLVector bMin = b.getMinimum();
|
|
MLVector bMax = b.getMaximum();
|
|
|
|
return (x >= bMin.x && x <= bMax.x && y >= bMin.y && y <= bMax.y && z
|
|
>= bMin.z && z <= bMax.z);
|
|
*/
|
|
return false;
|
|
}
|
|
|
|
void MLVector::setX(double x)
|
|
{
|
|
m_x = x;
|
|
}
|
|
|
|
double MLVector::getX() const
|
|
{
|
|
return m_x;
|
|
}
|
|
|
|
double& MLVector::getX()
|
|
{
|
|
return m_x;
|
|
}
|
|
|
|
void MLVector::setY(double y)
|
|
{
|
|
m_y = y;
|
|
}
|
|
|
|
double MLVector::getY() const
|
|
{
|
|
return m_y;
|
|
}
|
|
|
|
double& MLVector::getY()
|
|
{
|
|
return m_y;
|
|
}
|
|
|
|
void MLVector::setZ(double z)
|
|
{
|
|
m_z = z;
|
|
}
|
|
|
|
double MLVector::getZ() const
|
|
{
|
|
return m_z;
|
|
}
|
|
|
|
double& MLVector::getZ()
|
|
{
|
|
return m_z;
|
|
}
|
|
|
|
void MLVector::setValid(bool valid)
|
|
{
|
|
m_valid = valid;
|
|
}
|
|
|
|
bool MLVector::getValid() const
|
|
{
|
|
return m_valid;
|
|
}
|
|
|
|
/**
|
|
* \return True if this vector and the given vector are almost equal
|
|
* (see RS::PointTolerance).
|
|
*
|
|
* \param tol Tolerance in X, Y and Z.
|
|
|
|
bool MLVector::equalsFuzzy(const MLVector& v, double tol) const
|
|
{
|
|
return (fabs(x-v.x)<tol &&
|
|
fabs(y-v.y)<tol &&
|
|
fabs(z-v.z)<tol);
|
|
} */
|
|
|
|
/**
|
|
* \return The distance between this and the given coordinate.
|
|
*/
|
|
double MLVector::getDistanceTo(const MLVector& v) const
|
|
{
|
|
if (!valid || !v.valid) {
|
|
return RMAXDOUBLE;
|
|
} else {
|
|
return (*this - v).getMagnitude();
|
|
}
|
|
}
|
|
|
|
/**
|
|
* \return The distance between this and the given coordinate on the XY plane.
|
|
*/
|
|
double MLVector::getDistanceTo2d(const MLVector& v) const
|
|
{
|
|
if (!valid || !v.valid) {
|
|
return RMAXDOUBLE;
|
|
} else {
|
|
return (*this - v).getMagnitude2d();
|
|
}
|
|
}
|
|
|
|
/**
|
|
* \return true if this vector is within the given range (2d).
|
|
*/
|
|
bool MLVector::isInWindow(const MLVector& firstCorner,
|
|
const MLVector& secondCorner)
|
|
{
|
|
double minX = MIN(firstCorner.x, secondCorner.x);
|
|
double maxX = MAX(firstCorner.x, secondCorner.x);
|
|
double minY = MIN(firstCorner.y, secondCorner.y);
|
|
double maxY = MAX(firstCorner.y, secondCorner.y);
|
|
|
|
return (x >= minX && x <= maxX && y >= minY && y <= maxY);
|
|
}
|
|
|
|
|
|
void MLVector::setAngle(double a)
|
|
{
|
|
double m = getMagnitude();
|
|
setPolar(m, a);
|
|
}
|
|
|
|
/**
|
|
* \return The angle from zero to this vector (in rad).
|
|
*/
|
|
double MLVector::getAngle() const
|
|
{
|
|
double ret = 0.0;
|
|
double m = getMagnitude2d();
|
|
|
|
if (m > 1.0e-6)
|
|
{
|
|
double dp = getDotProduct(*this, MLVector(1.0, 0.0));
|
|
if (dp / m >= 1.0)
|
|
{
|
|
ret = 0.0;
|
|
}
|
|
else if (dp / m < -1.0)
|
|
{
|
|
ret = M_PI;
|
|
}
|
|
else
|
|
{
|
|
ret = acos(dp / m);
|
|
}
|
|
|
|
if (y < 0.0)
|
|
{
|
|
ret = 2*M_PI - ret;
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* \return Angle between this vector and XY plane (horizontal plane).
|
|
*/
|
|
double MLVector::getAngleToPlaneXY() const
|
|
{
|
|
MLVector n(0, 0, 1);
|
|
|
|
if (getMagnitude() < 1.0e-4)
|
|
{
|
|
return M_PI / 2;
|
|
}
|
|
else if ((getDotProduct(*this, n) / (getMagnitude() * 1)) > 1.0)
|
|
{
|
|
return 0.0;
|
|
}
|
|
else
|
|
{
|
|
return M_PI / 2 - acos(getDotProduct(*this, n) / (getMagnitude() * 1));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* \return The angle from this and the given coordinate (in rad).
|
|
*/
|
|
double MLVector::getAngleTo(const MLVector& v) const
|
|
{
|
|
if (!valid || !v.valid)
|
|
{
|
|
return RNANDOUBLE;
|
|
}
|
|
else
|
|
{
|
|
return (v - (*this)).getAngle();
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Sets the vector magnitude without chaning the direction.
|
|
*/
|
|
void MLVector::setMagnitude2d(double m)
|
|
{
|
|
double a = getAngle();
|
|
setPolar(m, a);
|
|
}
|
|
|
|
/**
|
|
* \return Magnitude (length) of the vector.
|
|
*/
|
|
double MLVector::getMagnitude() const
|
|
{
|
|
if (!valid)
|
|
{
|
|
return RNANDOUBLE;
|
|
}
|
|
// Note that the z coordinate is also needed for 2d
|
|
// (due to definition of crossP())
|
|
return 0;// sqrt(MLMath::pow(x, 2) + MLMath::pow(y, 2) + MLMath::pow(z, 2));
|
|
}
|
|
|
|
/**
|
|
* \return Magnitude (length) of the vector projected to the x/y plane (2d).
|
|
*/
|
|
double MLVector::getMagnitude2d() const
|
|
{
|
|
if (!valid)
|
|
{
|
|
return RNANDOUBLE;
|
|
}
|
|
return 0;// sqrt(MLMath::pow(x, 2) + MLMath::pow(y, 2));
|
|
}
|
|
|
|
/**
|
|
* \return Square of magnitude (length).
|
|
*/
|
|
double MLVector::getSquaredMagnitude() const
|
|
{
|
|
if (!valid)
|
|
{
|
|
return RNANDOUBLE;
|
|
}
|
|
|
|
return 0;// MLMath::pow(x, 2) + MLMath::pow(y, 2) + MLMath::pow(z, 2);
|
|
}
|
|
|
|
/**
|
|
* Linear interpolation between this and \c v by fraction 't'.
|
|
*/
|
|
MLVector MLVector::getLerp(const MLVector& v, double t) const
|
|
{
|
|
return MLVector(x + (v.x - x) * t, y + (v.y - y) * t, z + (v.z - z) * t);
|
|
}
|
|
|
|
/**
|
|
* \return Unit vector for this vector.
|
|
*/
|
|
MLVector MLVector::getUnitVector() const
|
|
{
|
|
return *this / getMagnitude();
|
|
}
|
|
|
|
/**
|
|
* binary + operator.
|
|
*/
|
|
MLVector MLVector::operator +(const MLVector& v) const
|
|
{
|
|
return MLVector(x + v.x, y + v.y, z + v.z, valid && v.valid);
|
|
}
|
|
|
|
/**
|
|
* binary - operator.
|
|
*/
|
|
MLVector MLVector::operator -(const MLVector& v) const
|
|
{
|
|
return MLVector(x - v.x, y - v.y, z - v.z, valid && v.valid);
|
|
}
|
|
|
|
/**
|
|
* binary * operator.
|
|
*/
|
|
MLVector MLVector::operator *(double s) const
|
|
{
|
|
return MLVector(x * s, y * s, z * s, valid);
|
|
}
|
|
|
|
/**
|
|
* binary / operator.
|
|
*/
|
|
MLVector MLVector::operator /(double s) const
|
|
{
|
|
return MLVector(x / s, y / s, z / s, valid);
|
|
}
|
|
|
|
/**
|
|
* unary - operator.
|
|
*/
|
|
MLVector MLVector::operator -() const
|
|
{
|
|
return getNegated();
|
|
}
|
|
|
|
/**
|
|
* \return New vector with negated components.
|
|
*/
|
|
MLVector MLVector::getNegated() const
|
|
{
|
|
return MLVector(-x, -y, -z, valid);
|
|
}
|
|
|
|
/**
|
|
* Normalizes this vector and returns a reference to this vector.
|
|
*/
|
|
MLVector MLVector::normalize()
|
|
{
|
|
*this = getNormalized();
|
|
return *this;
|
|
}
|
|
|
|
/**
|
|
* \return A new unit vector with the same direction as this vector.
|
|
*/
|
|
MLVector MLVector::getNormalized() const
|
|
{
|
|
/*
|
|
double l = getMagnitude();
|
|
if (l<MLCore::PointTolerance)
|
|
{
|
|
return MLVector::invalid;
|
|
}
|
|
return *this / l;*/
|
|
return *this;
|
|
}
|
|
|
|
/**
|
|
* += operator. The result is only valid if both vectors are valid.
|
|
*/
|
|
void MLVector::operator +=(const MLVector& v)
|
|
{
|
|
x += v.x;
|
|
y += v.y;
|
|
z += v.z;
|
|
valid = valid && v.valid;
|
|
}
|
|
|
|
/**
|
|
* -= operator. The result is only valid if both vectors are valid.
|
|
*/
|
|
void MLVector::operator -=(const MLVector& v)
|
|
{
|
|
x -= v.x;
|
|
y -= v.y;
|
|
z -= v.z;
|
|
valid = valid && v.valid;
|
|
}
|
|
|
|
/**
|
|
* *= operator
|
|
*/
|
|
void MLVector::operator *=(double s)
|
|
{
|
|
x *= s;
|
|
y *= s;
|
|
z *= s;
|
|
}
|
|
|
|
/**
|
|
* /= operator
|
|
*/
|
|
void MLVector::operator /=(double s)
|
|
{
|
|
x /= s;
|
|
y /= s;
|
|
z /= s;
|
|
}
|
|
|
|
/**
|
|
* == operator
|
|
*/
|
|
bool MLVector::operator ==(const MLVector& v) const
|
|
{
|
|
if (valid == true && v.valid == true)
|
|
{
|
|
return x == v.x && y == v.y && z == v.z;
|
|
}
|
|
else if (valid == false && v.valid == false)
|
|
{
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool MLVector::operator !=(const MLVector& v) const
|
|
{
|
|
return !operator==(v);
|
|
}
|
|
|
|
/**
|
|
* Scalarproduct (dot product).
|
|
*/
|
|
double MLVector::getDotProduct(const MLVector& v1, const MLVector& v2)
|
|
{
|
|
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
|
|
}
|
|
|
|
/**
|
|
* \return Cross product of two vectors.
|
|
*/
|
|
MLVector MLVector::getCrossProduct(const MLVector& v1, const MLVector& v2)
|
|
{
|
|
return MLVector(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x
|
|
* v2.y - v1.y * v2.x, v1.valid && v2.valid);
|
|
}
|
|
|
|
MLVector MLVector::createPolar(double radius, double angle)
|
|
{
|
|
MLVector ret;
|
|
ret.setPolar(radius, angle);
|
|
return ret;
|
|
}
|
|
|
|
|
|
/**
|
|
* \return A vector with the minimum components from the vectors v1 and v2.
|
|
* These might be mixed components from both vectors.
|
|
*/
|
|
MLVector MLVector::getMinimum(const MLVector& v1, const MLVector& v2)
|
|
{
|
|
return MLVector(MIN(v1.x, v2.x), MIN(v1.y, v2.y), MIN(v1.z, v2.z), v1.valid && v2.valid);
|
|
}
|
|
|
|
/**
|
|
* \return A vector with the maximum values from the vectors v1 and v2
|
|
*/
|
|
MLVector MLVector::getMaximum(const MLVector& v1, const MLVector& v2)
|
|
{
|
|
return MLVector(MAX(v1.x, v2.x), MAX(v1.y, v2.y), MAX(v1.z, v2.z), v1.valid && v2.valid);
|
|
}
|
|
|
|
/**
|
|
* Convenience function.
|
|
* \return (v1 + v2) / 2.0
|
|
*/
|
|
MLVector MLVector::getAverage(const MLVector& v1, const MLVector& v2)
|
|
{
|
|
return (v1+v2)/2.0;
|
|
}
|
|
|
|
/**
|
|
* Rotates this vector around 0/0 by the given angle.
|
|
*/
|
|
MLVector MLVector::rotate(double rotation)
|
|
{
|
|
if (!valid)
|
|
{
|
|
return *this;
|
|
}
|
|
|
|
double r = getMagnitude2d();
|
|
double a = getAngle() + rotation;
|
|
|
|
x = cos(a) * r;
|
|
y = sin(a) * r;
|
|
|
|
return *this;
|
|
}
|
|
|
|
/**
|
|
* Rotates this vector around the given center by the given angle.
|
|
*/
|
|
MLVector MLVector::rotate(double rotation, const MLVector& center)
|
|
{
|
|
*this = center + (*this - center).rotate(rotation);
|
|
return *this;
|
|
}
|
|
|
|
|
|
/**
|
|
* Scales this vector by the given factor with the given center.
|
|
*/
|
|
MLVector MLVector::scale(double factor, const MLVector& center)
|
|
{
|
|
return scale(MLVector(factor, factor, factor), center);
|
|
}
|
|
|
|
/**
|
|
* Scales this vector by the given factors with the given center.
|
|
*/
|
|
MLVector MLVector::scale(const MLVector& factors, const MLVector& center)
|
|
{
|
|
if (center==MLVector())
|
|
{
|
|
x *= factors.x;
|
|
y *= factors.y;
|
|
z *= factors.z;
|
|
return *this;
|
|
}
|
|
|
|
*this = center + (*this - center).scale(factors);
|
|
return *this;
|
|
}
|
|
|
|
|
|
/**
|
|
* Mirrors this vector at the given axis.
|
|
*/
|
|
MLVector MLVector::mirror(const MLLine& axis)
|
|
{
|
|
/* double phi1 = axis.startPoint.getAngleTo(*this);
|
|
double phi2 = axis.startPoint.getAngleTo(axis.endPoint) - phi1;
|
|
double r1 = axis.startPoint.getDistanceTo(*this);
|
|
double r2 = axis.endPoint.getDistanceTo(*this);
|
|
|
|
if (r1 < 1.0e-6 || r2 < 1.0e-6)
|
|
{
|
|
// point touches one axis point
|
|
}
|
|
else
|
|
{
|
|
setPolar(r1, phi1 + 2* phi2 );
|
|
(*this) += axis.startPoint;
|
|
}
|
|
*/
|
|
return *this;
|
|
}
|
|
|
|
MLVector MLVector::mirror(const MLVector& axis1, const MLVector& axis2)
|
|
{
|
|
return *this;
|
|
//return mirror(MLLine(axis1, axis2));
|
|
}
|
|
|
|
|
|
/**
|
|
* Mirrors this vector at the Y-axis.
|
|
*/
|
|
MLVector MLVector::flipHorizontal()
|
|
{
|
|
return mirror(MLVector(0,0,0), MLVector(0,1,0));
|
|
}
|
|
|
|
/**
|
|
* Mirrors this vector at the X-axis.
|
|
*/
|
|
MLVector MLVector::flipVertical()
|
|
{
|
|
return mirror(MLVector(0,0,0), MLVector(1,0,0));
|
|
} |