103 lines
2.4 KiB
C++
103 lines
2.4 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.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
#include "gazebo/math/Plane.hh"
|
||
|
|
|
||
|
|
using namespace gazebo;
|
||
|
|
using namespace math;
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
Plane::Plane()
|
||
|
|
{
|
||
|
|
this->d = 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
Plane::Plane(const Vector3 &_normal, double _offset)
|
||
|
|
{
|
||
|
|
this->normal = _normal;
|
||
|
|
this->d = _offset;
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
Plane::Plane(const Vector3 &_normal, const Vector2d &_size, double _offset)
|
||
|
|
{
|
||
|
|
this->Set(_normal, _size, _offset);
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
Plane::Plane(const ignition::math::Planed &_plane)
|
||
|
|
{
|
||
|
|
this->Set(_plane.Normal(), _plane.Size(), _plane.Offset());
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
Plane::~Plane()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void Plane::Set(const Vector3 &_n, const Vector2d &_s, double _offset)
|
||
|
|
{
|
||
|
|
this->normal = _n;
|
||
|
|
this->size = _s;
|
||
|
|
this->d = _offset;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
Plane &Plane::operator =(const Plane & _p)
|
||
|
|
{
|
||
|
|
this->normal = _p.normal;
|
||
|
|
this->size = _p.size;
|
||
|
|
this->d = _p.d;
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
Plane &Plane::operator=(const ignition::math::Planed &_p)
|
||
|
|
{
|
||
|
|
this->normal = _p.Normal();
|
||
|
|
this->size = _p.Size();
|
||
|
|
this->d = _p.Offset();
|
||
|
|
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
double Plane::Distance(const Vector3 &_origin, const Vector3 &_dir) const
|
||
|
|
{
|
||
|
|
double denom = this->normal.Dot(_dir);
|
||
|
|
|
||
|
|
if (fabs(denom) < 1e-3)
|
||
|
|
{
|
||
|
|
// parallel
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
double nom = _origin.Dot(this->normal) - this->d;
|
||
|
|
double t = -(nom/denom);
|
||
|
|
return t;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
ignition::math::Planed Plane::Ign() const
|
||
|
|
{
|
||
|
|
return ignition::math::Planed(this->normal.Ign(), this->size.Ign(), this->d);
|
||
|
|
}
|