Loading...
Searching...
No Matches
GeometricEquations.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2015, University of Toronto
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the University of Toronto nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Jonathan Gammell*/
36
37// This file's header
38#include "ompl/util/GeometricEquations.h"
39
40// For gamma function
41#include <cmath>
42
43// For pi definition
44#include <boost/math/constants/constants.hpp>
45
46// OMPL exceptions
47#include "ompl/util/Exception.h"
48
49double ompl::nBallMeasure(unsigned int N, double r)
50{
51 return std::pow(std::sqrt(boost::math::constants::pi<double>()) * r, static_cast<double>(N)) /
52 std::tgamma(static_cast<double>(N) / 2.0 + 1.0);
53}
54
55double ompl::unitNBallMeasure(unsigned int N)
56{
57 // This is the radius version with r removed (as it is 1) for efficiency
58 return std::pow(std::sqrt(boost::math::constants::pi<double>()), static_cast<double>(N)) /
59 std::tgamma(static_cast<double>(N) / 2.0 + 1.0);
60}
61
62double ompl::prolateHyperspheroidMeasure(unsigned int N, double dFoci, double dTransverse)
63{
64 // Sanity check input
65 if (dTransverse < dFoci)
66 {
67 throw Exception("Transverse diameter cannot be less than the minimum transverse diameter.");
68 }
69
70 // Variable
71 // The conjugate diameter:
72 double conjugateDiameter;
73 // The Lebesgue measure return value
74 double lmeas;
75
76 // Calculate the conjugate diameter:
77 conjugateDiameter = std::sqrt(dTransverse * dTransverse - dFoci * dFoci);
78
79 // Calculate the volume
80 // First multiply together the radii, noting that the first radius is the transverse diameter/2.0, and the other N-1
81 // are the conjugate diameter/2.0
82 lmeas = dTransverse / 2.0;
83 for (unsigned int i = 1u; i < N; ++i)
84 {
85 lmeas = lmeas * conjugateDiameter / 2.0;
86 }
87
88 // Then multiply by the volume of the unit n-ball.
89 lmeas = lmeas * unitNBallMeasure(N);
90
91 // Finally return:
92 return lmeas;
93}
The exception type for ompl.
Definition Exception.h:47
double nBallMeasure(unsigned int N, double r)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball of given radius.
double prolateHyperspheroidMeasure(unsigned int N, double dFoci, double dTransverse)
The Lebesgue measure (i.e., "volume") of an n-dimensional prolate hyperspheroid (a symmetric hyperell...
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.