230 lines
7.1 KiB
C++
230 lines
7.1 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.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
/* Desc: 4 tuple
|
||
|
|
* Author: Nate Koenig
|
||
|
|
* Date: 19 Aug 2008
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifndef _VECTOR4_HH_
|
||
|
|
#define _VECTOR4_HH_
|
||
|
|
|
||
|
|
#include <iostream>
|
||
|
|
#include <fstream>
|
||
|
|
#include "gazebo/math/Matrix4.hh"
|
||
|
|
#include "gazebo/util/system.hh"
|
||
|
|
|
||
|
|
namespace gazebo
|
||
|
|
{
|
||
|
|
namespace math
|
||
|
|
{
|
||
|
|
/// \addtogroup gazebo_math
|
||
|
|
/// \{
|
||
|
|
|
||
|
|
/// \class Vector4 Vector4.hh math/gzmath.hh
|
||
|
|
/// \brief double Generic x, y, z, w vector
|
||
|
|
class GZ_MATH_VISIBLE Vector4
|
||
|
|
{
|
||
|
|
/// \brief Constructor
|
||
|
|
public: Vector4();
|
||
|
|
|
||
|
|
/// \brief Constructor with component values
|
||
|
|
/// \param[in] _x value along x axis
|
||
|
|
/// \param[in] _y value along y axis
|
||
|
|
/// \param[in] _z value along z axis
|
||
|
|
/// \param[in] _w value along w axis
|
||
|
|
public: Vector4(const double &_x, const double &_y, const double &_z,
|
||
|
|
const double &_w);
|
||
|
|
|
||
|
|
/// \brief Copy constructor
|
||
|
|
/// \param[in] _v vector
|
||
|
|
public: Vector4(const Vector4 &_v);
|
||
|
|
|
||
|
|
|
||
|
|
/// \brief Destructor
|
||
|
|
public: virtual ~Vector4();
|
||
|
|
|
||
|
|
/// \brief Calc distance to the given point
|
||
|
|
/// \param[in] _pt the point
|
||
|
|
/// \return the distance
|
||
|
|
public: double Distance(const Vector4 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Returns the length (magnitude) of the vector
|
||
|
|
public: double GetLength() const;
|
||
|
|
|
||
|
|
/// \brief Return the square of the length (magnitude) of the vector
|
||
|
|
/// \return the length
|
||
|
|
public: double GetSquaredLength() const;
|
||
|
|
|
||
|
|
/// \brief Normalize the vector length
|
||
|
|
public: void Normalize();
|
||
|
|
|
||
|
|
/// \brief Set the contents of the vector
|
||
|
|
/// \param[in] _x value along x axis
|
||
|
|
/// \param[in] _y value along y axis
|
||
|
|
/// \param[in] _z value along z axis
|
||
|
|
/// \param[in] _w value along w axis
|
||
|
|
public: void Set(double _x = 0, double _y = 0 , double _z = 0,
|
||
|
|
double _w = 0);
|
||
|
|
|
||
|
|
/// \brief Assignment operator
|
||
|
|
/// \param[in] _v the vector
|
||
|
|
/// \return a reference to this vector
|
||
|
|
public: Vector4 &operator =(const Vector4 &_v);
|
||
|
|
|
||
|
|
/// \brief Assignment operator
|
||
|
|
/// \param[in] _value
|
||
|
|
public: Vector4 &operator =(double _value);
|
||
|
|
|
||
|
|
/// \brief Addition operator
|
||
|
|
/// \param[in] _v the vector to add
|
||
|
|
/// \result a sum vector
|
||
|
|
public: Vector4 operator+(const Vector4 &_v) const;
|
||
|
|
|
||
|
|
/// \brief Addition operator
|
||
|
|
/// \param[in] _v the vector to add
|
||
|
|
/// \return this vector
|
||
|
|
public: const Vector4 &operator+=(const Vector4 &_v);
|
||
|
|
|
||
|
|
/// \brief Subtraction operator
|
||
|
|
/// \param[in] _v the vector to substract
|
||
|
|
/// \return a vector
|
||
|
|
public: Vector4 operator-(const Vector4 &_v) const;
|
||
|
|
|
||
|
|
/// \brief Subtraction assigment operators
|
||
|
|
/// \param[in] _v the vector to substract
|
||
|
|
/// \return this vector
|
||
|
|
public: const Vector4 &operator-=(const Vector4 &_v);
|
||
|
|
|
||
|
|
/// \brief Division assignment operator
|
||
|
|
/// \remarks Performs element wise division,
|
||
|
|
/// which has limited use.
|
||
|
|
/// \param[in] _v the vector to perform element wise division with
|
||
|
|
/// \return a result vector
|
||
|
|
public: const Vector4 operator/(const Vector4 &_v) const;
|
||
|
|
|
||
|
|
/// \brief Division assignment operator
|
||
|
|
/// \remarks Performs element wise division,
|
||
|
|
/// which has limited use.
|
||
|
|
/// \param[in] _v the vector to perform element wise division with
|
||
|
|
/// \return this
|
||
|
|
public: const Vector4 &operator/=(const Vector4 &_v);
|
||
|
|
|
||
|
|
/// \brief Division assignment operator
|
||
|
|
/// \remarks Performs element wise division,
|
||
|
|
/// which has limited use.
|
||
|
|
/// \param[in] _pt another vector
|
||
|
|
/// \return a result vector
|
||
|
|
public: const Vector4 operator/(double _v) const;
|
||
|
|
|
||
|
|
/// \brief Division operator
|
||
|
|
/// \param[in] _v scaling factor
|
||
|
|
/// \return a vector
|
||
|
|
public: const Vector4 &operator/=(double _v);
|
||
|
|
|
||
|
|
/// \brief Multiplication operator.
|
||
|
|
/// \remarks Performs element wise multiplication,
|
||
|
|
/// which has limited use.
|
||
|
|
/// \param[in] _pt another vector
|
||
|
|
/// \return result vector
|
||
|
|
public: const Vector4 operator*(const Vector4 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Matrix multiplication operator.
|
||
|
|
/// \param[in] _m matrix
|
||
|
|
/// \return the vector multiplied by _m
|
||
|
|
public: const Vector4 operator*(const Matrix4 &_m) const;
|
||
|
|
|
||
|
|
/// \brief Multiplication assignment operator
|
||
|
|
/// \remarks Performs element wise multiplication,
|
||
|
|
/// which has limited use.
|
||
|
|
/// \param[in] _pt a vector
|
||
|
|
/// \return this
|
||
|
|
public: const Vector4 &operator*=(const Vector4 &_pt);
|
||
|
|
|
||
|
|
/// \brief Multiplication operators
|
||
|
|
/// \param[in] _v scaling factor
|
||
|
|
/// \return a scaled vector
|
||
|
|
public: const Vector4 operator*(double _v) const;
|
||
|
|
|
||
|
|
/// \brief Multiplication assignment operator
|
||
|
|
/// \param[in] _v scaling factor
|
||
|
|
/// \return this
|
||
|
|
public: const Vector4 &operator*=(double _v);
|
||
|
|
|
||
|
|
/// \brief Equal to operator
|
||
|
|
/// \param[in] _pt the other vector
|
||
|
|
/// \return true if each component is equal withing a
|
||
|
|
/// default tolerence (1e-6), false otherwise
|
||
|
|
public: bool operator ==(const Vector4 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief Not equal to operator
|
||
|
|
/// \param[in] _pt the other vector
|
||
|
|
/// \return true if each component is equal withing a
|
||
|
|
/// default tolerence (1e-6), false otherwise
|
||
|
|
public: bool operator!=(const Vector4 &_pt) const;
|
||
|
|
|
||
|
|
/// \brief See if a point is finite (e.g., not nan)
|
||
|
|
/// \return true if finite, false otherwise
|
||
|
|
public: bool IsFinite() const;
|
||
|
|
|
||
|
|
/// \brief Array subscript operator
|
||
|
|
/// \param[in] _index
|
||
|
|
public: double operator[](unsigned int _index) const;
|
||
|
|
|
||
|
|
/// X value
|
||
|
|
public: double x;
|
||
|
|
|
||
|
|
/// Y value
|
||
|
|
public: double y;
|
||
|
|
|
||
|
|
/// Z value
|
||
|
|
public: double z;
|
||
|
|
|
||
|
|
/// W value
|
||
|
|
public: double w;
|
||
|
|
|
||
|
|
/// \brief Stream insertion operator
|
||
|
|
/// \param[in] _out output stream
|
||
|
|
/// \param[in] _pt Vector4 to output
|
||
|
|
/// \return The stream
|
||
|
|
public: friend std::ostream &operator<<(std::ostream &_out,
|
||
|
|
const gazebo::math::Vector4 &_pt)
|
||
|
|
{
|
||
|
|
_out << _pt.x << " " << _pt.y << " " << _pt.z << " " << _pt.w;
|
||
|
|
return _out;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief Stream extraction operator
|
||
|
|
/// \param[in] _in input stream
|
||
|
|
/// \param[in] _pt Vector4 to read values into
|
||
|
|
/// \return the stream
|
||
|
|
public: friend std::istream &operator >> (std::istream &_in,
|
||
|
|
gazebo::math::Vector4 &_pt)
|
||
|
|
{
|
||
|
|
// Skip white spaces
|
||
|
|
_in.setf(std::ios_base::skipws);
|
||
|
|
_in >> _pt.x >> _pt.y >> _pt.z >> _pt.w;
|
||
|
|
return _in;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
/// \}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
|
||
|
|
|