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 
49 double 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 
55 double 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 
62 double 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 }
double nBallMeasure(unsigned int N, double r)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball of given radius.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit 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...
The exception type for ompl.
Definition: Exception.h:78