348 lines
12 KiB
C++
348 lines
12 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
||
|
|
*
|
||
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||
|
|
* you may not use this file except in compliance with the License.
|
||
|
|
* You may obtain a copy of the License at
|
||
|
|
*
|
||
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||
|
|
*
|
||
|
|
* Unless required by applicable law or agreed to in writing, software
|
||
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
|
|
* See the License for the specific language governing permissions and
|
||
|
|
* limitations under the License.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
#ifndef _GAZEBO_VECTOR3_HH_
|
||
|
|
#define _GAZEBO_VECTOR3_HH_
|
||
|
|
|
||
|
|
#include <math.h>
|
||
|
|
#include <iostream>
|
||
|
|
#include <fstream>
|
||
|
|
#include <ignition/math/Vector3.hh>
|
||
|
|
|
||
|
|
#include "gazebo/math/Helpers.hh"
|
||
|
|
#include "gazebo/util/system.hh"
|
||
|
|
|
||
|
|
namespace gazebo
|
||
|
|
{
|
||
|
|
namespace math
|
||
|
|
{
|
||
|
|
/// \addtogroup gazebo_math
|
||
|
|
/// \{
|
||
|
|
|
||
|
|
/// \class Vector3 Vector3.hh math/gzmath.hh
|
||
|
|
/// \brief The Vector3 class represents the generic vector containing 3
|
||
|
|
/// elements. Since it's commonly used to keep coordinate system
|
||
|
|
/// related information, its elements are labeled by x, y, z.
|
||
|
|
class GZ_MATH_VISIBLE Vector3
|
||
|
|
{
|
||
|
|
/// \brief math::Vector3(0, 0, 0)
|
||
|
|
public: static const Vector3 Zero;
|
||
|
|
|
||
|
|
/// \brief math::Vector3(1, 1, 1)
|
||
|
|
public: static const Vector3 One;
|
||
|
|
|
||
|
|
/// \brief math::Vector3(1, 0, 0)
|
||
|
|
public: static const Vector3 UnitX;
|
||
|
|
|
||
|
|
/// \brief math::Vector3(0, 1, 0)
|
||
|
|
public: static const Vector3 UnitY;
|
||
|
|
|
||
|
|
/// \brief math::Vector3(0, 0, 1)
|
||
|
|
public: static const Vector3 UnitZ;
|
||
|
|
|
||
|
|
/// \brief Constructor
|
||
|
|
public: Vector3();
|
||
|
|
|
||
|
|
/// \brief Constructor
|
||
|
|
/// \param[in] _x value along x
|
||
|
|
/// \param[in] _y value along y
|
||
|
|
/// \param[in] _z value along z
|
||
|
|
public: Vector3(const double &_x, const double &_y, const double &_z);
|
||
|
|
|
||
|
|
/// \brief Ignition math copy constructor
|
||
|
|
/// \param[in] _v a vector
|
||
|
|
public: Vector3(const ignition::math::Vector3d &_v);
|
||
|
|
|
||
|
|
/// \brief Copy constructor
|
||
|
|
/// \param[in] _v a vector
|
||
|
|
public: Vector3(const Vector3 &_v);
|
||
|
|
|
||
|
|
/// \brief Destructor
|
||
|
|
public: virtual ~Vector3();
|
||
|
|
|
||
|
|
/// \brief Return the sum of the values
|
||
|
|
/// \return the sum
|
||
|
|
public: double GetSum() const;
|
||
|
|
|
||
|
|
/// \brief Calc distance to the given point
|
||
|
|
/// \param[in] _pt the point
|
||
|
|
/// \return the distance
|
||
|
|
public: double Distance(const Vector3 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Calc distance to the given point
|
||
|
|
/// \param[in] _x value along x
|
||
|
|
/// \param[in] _y value along y
|
||
|
|
/// \param[in] _z value along z
|
||
|
|
/// \return the distance
|
||
|
|
public: double Distance(double _x, double _y, double _z) const;
|
||
|
|
|
||
|
|
/// \brief Returns the length (magnitude) of the vector
|
||
|
|
/// \ return the length
|
||
|
|
public: double GetLength() const;
|
||
|
|
|
||
|
|
/// \brief Return the square of the length (magnitude) of the vector
|
||
|
|
/// \return the squared length
|
||
|
|
public: double GetSquaredLength() const;
|
||
|
|
|
||
|
|
/// \brief Normalize the vector length
|
||
|
|
/// \return unit length vector
|
||
|
|
public: Vector3 Normalize();
|
||
|
|
|
||
|
|
/// \brief Round to near whole number, return the result.
|
||
|
|
/// \return the result
|
||
|
|
public: Vector3 Round();
|
||
|
|
|
||
|
|
/// \brief Get a rounded version of this vector
|
||
|
|
/// \return a rounded vector
|
||
|
|
public: Vector3 GetRounded() const;
|
||
|
|
|
||
|
|
/// \brief Set the contents of the vector
|
||
|
|
/// \param[in] _x value along x
|
||
|
|
/// \param[in] _y value along y
|
||
|
|
/// \param[in] _z value aling z
|
||
|
|
public: inline void Set(double _x = 0, double _y = 0 , double _z = 0)
|
||
|
|
{
|
||
|
|
this->x = _x;
|
||
|
|
this->y = _y;
|
||
|
|
this->z = _z;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief Return the cross product of this vector and pt
|
||
|
|
/// \return the product
|
||
|
|
public: Vector3 Cross(const Vector3 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Return the dot product of this vector and pt
|
||
|
|
/// \return the product
|
||
|
|
public: double Dot(const Vector3 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Get the absolute value of the vector
|
||
|
|
/// \return a vector with positive elements
|
||
|
|
public: Vector3 GetAbs() const;
|
||
|
|
|
||
|
|
/// \brief Return a vector that is perpendicular to this one.
|
||
|
|
/// \return an orthogonal vector
|
||
|
|
public: Vector3 GetPerpendicular() const;
|
||
|
|
|
||
|
|
/// \brief Get a normal vector to a triangle
|
||
|
|
/// \param[in] _v1 first vertex of the triangle
|
||
|
|
/// \param[in] _v2 second vertex
|
||
|
|
/// \param[in] _v3 third vertex
|
||
|
|
/// \return the normal
|
||
|
|
public: static Vector3 GetNormal(const Vector3 &_v1, const Vector3 &_v2,
|
||
|
|
const Vector3 &_v3);
|
||
|
|
|
||
|
|
/// \brief Get distance to a line
|
||
|
|
/// \param[in] _pt1 first point on the line
|
||
|
|
/// \param[in] _pt2 second point on the line
|
||
|
|
/// \return the minimum distance from this point to the line
|
||
|
|
public: double GetDistToLine(const Vector3 &_pt1, const Vector3 &_pt2);
|
||
|
|
|
||
|
|
/// \brief Set this vector's components to the maximum of itself and the
|
||
|
|
/// passed in vector
|
||
|
|
/// \param[in] _v the maximum clamping vector
|
||
|
|
public: void SetToMax(const Vector3 &_v);
|
||
|
|
|
||
|
|
/// \brief Set this vector's components to the minimum of itself and the
|
||
|
|
/// passed in vector
|
||
|
|
/// \param[in] _v the minimum clamping vector
|
||
|
|
public: void SetToMin(const Vector3 &_v);
|
||
|
|
|
||
|
|
/// \brief Get the maximum value in the vector
|
||
|
|
/// \return the maximum element
|
||
|
|
public: double GetMax() const;
|
||
|
|
|
||
|
|
/// \brief Get the minimum value in the vector
|
||
|
|
/// \return the minimum element
|
||
|
|
public: double GetMin() const;
|
||
|
|
|
||
|
|
/// \brief Convert this vector to an ignition::math::Vector3d.
|
||
|
|
/// \return This vector as an ignition::math::Vector3d.
|
||
|
|
public: ignition::math::Vector3d Ign() const;
|
||
|
|
|
||
|
|
/// \brief Assignment operator for ignition math
|
||
|
|
/// \param[in] _v a new value
|
||
|
|
/// \return this
|
||
|
|
public: Vector3 &operator=(const ignition::math::Vector3d &_v);
|
||
|
|
|
||
|
|
/// \brief Assignment operator
|
||
|
|
/// \param[in] _v a new value
|
||
|
|
/// \return The new vector
|
||
|
|
public: Vector3 &operator =(const Vector3 &_v);
|
||
|
|
|
||
|
|
/// \brief Assignment operator
|
||
|
|
/// \param[in] _value assigned to all elements
|
||
|
|
/// \return this
|
||
|
|
public: Vector3 &operator =(double _value);
|
||
|
|
|
||
|
|
/// \brief Addition operator
|
||
|
|
/// \param[in] _v vector to add
|
||
|
|
/// \return the sum vector
|
||
|
|
public: Vector3 operator+(const Vector3 &_v) const;
|
||
|
|
|
||
|
|
/// \brief Addition assignment operator
|
||
|
|
/// \param[in] _v vector to add
|
||
|
|
public: const Vector3 &operator+=(const Vector3 &_v);
|
||
|
|
|
||
|
|
/// \brief Negation operator
|
||
|
|
/// \return negative of this vector
|
||
|
|
public: inline Vector3 operator-() const
|
||
|
|
{
|
||
|
|
return Vector3(-this->x, -this->y, -this->z);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief Subtraction operators
|
||
|
|
/// \param[in] _pt a vector to substract
|
||
|
|
/// \return a vector
|
||
|
|
public: inline Vector3 operator-(const Vector3 &_pt) const
|
||
|
|
{
|
||
|
|
return Vector3(this->x - _pt.x,
|
||
|
|
this->y - _pt.y,
|
||
|
|
this->z - _pt.z);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief Subtraction operators
|
||
|
|
/// \param[in] _pt subtrahend
|
||
|
|
public: const Vector3 &operator-=(const Vector3 &_pt);
|
||
|
|
|
||
|
|
/// \brief Division operator
|
||
|
|
/// \brief[in] _pt the vector divisor
|
||
|
|
/// \remarks this is an element wise division
|
||
|
|
/// \return a vector
|
||
|
|
public: const Vector3 operator/(const Vector3 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Division assignment operator
|
||
|
|
/// \brief[in] _pt the vector divisor
|
||
|
|
/// \remarks this is an element wise division
|
||
|
|
/// \return a vector
|
||
|
|
public: const Vector3 &operator/=(const Vector3 &_pt);
|
||
|
|
|
||
|
|
/// \brief Division operator
|
||
|
|
/// \remarks this is an element wise division
|
||
|
|
/// \return a vector
|
||
|
|
public: const Vector3 operator/(double _v) const;
|
||
|
|
|
||
|
|
/// \brief Division operator
|
||
|
|
/// \remarks this is an element wise division
|
||
|
|
/// \return this
|
||
|
|
public: const Vector3 &operator/=(double _v);
|
||
|
|
|
||
|
|
/// \brief Multiplication operator
|
||
|
|
/// \remarks this is an element wise multiplication, not a cross product
|
||
|
|
/// \param[in] _v
|
||
|
|
public: Vector3 operator*(const Vector3 &_p) const;
|
||
|
|
|
||
|
|
/// \brief Multiplication operators
|
||
|
|
/// \remarks this is an element wise multiplication, not a cross product
|
||
|
|
/// \param[in] _v a vector
|
||
|
|
/// \return this
|
||
|
|
public: const Vector3 &operator*=(const Vector3 &_v);
|
||
|
|
|
||
|
|
/// \brief Multiplication operators
|
||
|
|
/// \param[in] _s the scaling factor
|
||
|
|
/// \param[in] _v input vector
|
||
|
|
/// \return a scaled vector
|
||
|
|
public: friend inline Vector3 operator*(double _s,
|
||
|
|
const Vector3 &_v)
|
||
|
|
{ return Vector3(_v.x * _s, _v.y * _s, _v.z * _s); }
|
||
|
|
|
||
|
|
/// \brief Multiplication operators
|
||
|
|
/// \param[in] _v the scaling factor
|
||
|
|
/// \return a scaled vector
|
||
|
|
public: Vector3 operator*(double _v) const;
|
||
|
|
|
||
|
|
/// \brief Multiplication operator
|
||
|
|
/// \param[in] _v scaling factor
|
||
|
|
/// \return this
|
||
|
|
public: const Vector3 &operator*=(double _v);
|
||
|
|
|
||
|
|
/// \brief Equal to operator
|
||
|
|
/// \param[in] _pt The vector to compare against
|
||
|
|
/// \return true if each component is equal withing a
|
||
|
|
/// default tolerence (1e-6), false otherwise
|
||
|
|
public: bool operator ==(const Vector3 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Not equal to operator
|
||
|
|
/// \param[in] _v The vector to compare against
|
||
|
|
/// \return true if each component is equal withing a
|
||
|
|
/// default tolerence (1e-6), false otherwise
|
||
|
|
public: bool operator!=(const Vector3 &_v) const;
|
||
|
|
|
||
|
|
/// \brief See if a point is finite (e.g., not nan)
|
||
|
|
public: bool IsFinite() const;
|
||
|
|
|
||
|
|
/// \brief Corrects any nan values
|
||
|
|
public: inline void Correct()
|
||
|
|
{
|
||
|
|
if (!std::isfinite(this->x))
|
||
|
|
this->x = 0;
|
||
|
|
if (!std::isfinite(this->y))
|
||
|
|
this->y = 0;
|
||
|
|
if (!std::isfinite(this->z))
|
||
|
|
this->z = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief [] operator
|
||
|
|
public: double operator[](unsigned int index) const;
|
||
|
|
|
||
|
|
/// \brief Round all values to _precision decimal places
|
||
|
|
/// \param[in] _precision the decimal places
|
||
|
|
public: void Round(int _precision);
|
||
|
|
|
||
|
|
/// \brief Equality test
|
||
|
|
/// \remarks This is equivalent to the == operator
|
||
|
|
/// \param[in] _v the other vector
|
||
|
|
/// \return true if the 2 vectors have the same values, false otherwise
|
||
|
|
public: bool Equal(const Vector3 &_v) const;
|
||
|
|
|
||
|
|
/// \brief X location
|
||
|
|
public: double x;
|
||
|
|
|
||
|
|
/// \brief Y location
|
||
|
|
public: double y;
|
||
|
|
|
||
|
|
/// \brief Z location
|
||
|
|
public: double z;
|
||
|
|
|
||
|
|
/// \brief Stream insertion operator
|
||
|
|
/// \param _out output stream
|
||
|
|
/// \param _pt Vector3 to output
|
||
|
|
/// \return the stream
|
||
|
|
public: friend std::ostream &operator<<(std::ostream &_out,
|
||
|
|
const gazebo::math::Vector3 &_pt)
|
||
|
|
{
|
||
|
|
_out << precision(_pt.x, 6) << " " << precision(_pt.y, 6) << " "
|
||
|
|
<< precision(_pt.z, 6);
|
||
|
|
return _out;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief Stream extraction operator
|
||
|
|
/// \param _in input stream
|
||
|
|
/// \param _pt vector3 to read values into
|
||
|
|
/// \return the stream
|
||
|
|
public: friend std::istream &operator>>(std::istream &_in,
|
||
|
|
gazebo::math::Vector3 &_pt)
|
||
|
|
{
|
||
|
|
// Skip white spaces
|
||
|
|
_in.setf(std::ios_base::skipws);
|
||
|
|
_in >> _pt.x >> _pt.y >> _pt.z;
|
||
|
|
return _in;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
/// \}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|