polyhedron_volume.hh 4.95 KB
``````/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*
* Unless required by applicable law or agreed to in writing, software
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
*
*/

#pragma once

#include <cassert>
#include <vector>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>

namespace buoyancy
{
/// \brief Represents a plane as a normal and offset
struct Plane
{
/// \brief Initializes plane at z=0
Plane();

/// \brief Vector3 normal to plane
ignition::math::Vector3d normal;

/// \brief Offset w.r.t. normal
float offset;
};

/// \brief Represents output volume with centroid
struct Volume
{
Volume();

/// \brief Overloads += for volume object
Volume& operator+=(const Volume& rhs);

/// \brief Submerged volume of shape
double volume;

/// \brief Vector3 representing volume centroid
ignition::math::Vector3d centroid;
};

/// \brief Submerged volume calculation using polyhedron
/// based on: Exact Buoyancy for Polyhedra by Eric Catto
class Polyhedron
{
/// \brief Store vertex index for a triangular face
public: struct Face
{
Face() = default;

Face(int i1, int i2, int i3);

/// \brief Index of vertices
int i1, i2, i3;
};

/// \brief Generate a cube polyhedron centered at origin
/// @param x: length of cube
/// @param y: width of cube
/// @param z: height of cube
/// @return Polyhedron object
public: static Polyhedron makeCube(double x,
double y,
double z);

/// \brief Generate a cylinder polyhedron centered at origin
/// @param r: radius of cylinder
/// @param l: length of cylinder
/// @param n: number of segments
/// @return Polyhedron object
public: static Polyhedron makeCylinder(double r,
double l,
int n);

/// \brief Compute full volume and center of buoyancy of the polyhedron
/// @return Volume object with volume and centroid
public: Volume ComputeFullVolume();

/// \brief Compute submerge volume and center of buoyancy of a polyhedron
/// @param x: our position
/// @param q: our orientation (quaternions)
/// @param plane: water surface defined as a plane
/// @return Volume object with volume and centroid (relative to world)
public: Volume SubmergedVolume(const ignition::math::Vector3d &x,
const ignition::math::Quaterniond &q,
Plane &plane);

/// \brief Computes volume and centroid of tetrahedron
/// tetrahedron formed by triangle + arbitrary point
/// @param v1: point on triangle
/// @param v2: point on triangle
/// @param v3: point on triangle
/// @param p: arbitrary point
/// @return Volume object with volume and centroid
private: static Volume tetrahedronVolume(const ignition::math::Vector3d& v1,
const ignition::math::Vector3d& v2,
const ignition::math::Vector3d& v3,
const ignition::math::Vector3d& p =
ignition::math::Vector3d({0., 0., 0.}));

/// \brief Clips a partially submerged triangle
/// @param v1: point on triangle
/// @param v2: point on triangle
/// @param v3: point on triangle
/// @param d1: distance of point v1 to the splitting plane
/// @param d2: distance of point v2 to the splitting plane
/// @param d3: distance of point v3 to the splitting plane
/// @return Volume object for clipped tetrahedron
private: static Volume clipTriangle(const ignition::math::Vector3d& v1,
const ignition::math::Vector3d& v2,
const ignition::math::Vector3d& v3,
double d1,
double d2,
double d3,
const ignition::math::Vector3d& p =
ignition::math::Vector3d({0., 0., 0.}));

/// \brief Object vertices
private: std::vector<ignition::math::Vector3d> vertices;

/// \brief Object faces
private: std::vector<Face> faces;

/// \brief Values below this are zeroed out
private: const double EPSILON = 1e-6;
};  // class Polyhedron
}``````