diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp index dcdc07b60..6fcd67e16 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp @@ -3,7 +3,6 @@ #define _USE_MATH_DEFINES #include -cRand cMathUtil::gRand = cRand(); int cMathUtil::Clamp(int val, int min, int max) { @@ -45,69 +44,6 @@ double cMathUtil::NormalizeAngle(double theta) return norm_theta; } -double cMathUtil::RandDouble() -{ - return RandDouble(0, 1); -} - -double cMathUtil::RandDouble(double min, double max) -{ - return gRand.RandDouble(min, max); -} - -double cMathUtil::RandDoubleNorm(double mean, double stdev) -{ - return gRand.RandDoubleNorm(mean, stdev); -} - -double cMathUtil::RandDoubleExp(double lambda) -{ - return gRand.RandDoubleExp(lambda); -} - -double cMathUtil::RandDoubleSeed(double seed) -{ - unsigned int int_seed = *reinterpret_cast(&seed); - std::default_random_engine rand_gen(int_seed); - std::uniform_real_distribution dist; - return dist(rand_gen); -} - -int cMathUtil::RandInt() -{ - return gRand.RandInt(); -} - -int cMathUtil::RandInt(int min, int max) -{ - return gRand.RandInt(min, max); -} - -int cMathUtil::RandUint() -{ - return gRand.RandUint(); -} - -int cMathUtil::RandUint(unsigned int min, unsigned int max) -{ - return gRand.RandUint(min, max); -} - -int cMathUtil::RandIntExclude(int min, int max, int exc) -{ - return gRand.RandIntExclude(min, max, exc); -} - -void cMathUtil::SeedRand(unsigned long int seed) -{ - gRand.Seed(seed); - srand(gRand.RandInt()); -} - -int cMathUtil::RandSign() -{ - return gRand.RandSign(); -} double cMathUtil::SmoothStep(double t) { @@ -115,10 +51,6 @@ double cMathUtil::SmoothStep(double t) return val; } -bool cMathUtil::FlipCoin(double p) -{ - return gRand.FlipCoin(p); -} tMatrix cMathUtil::TranslateMat(const tVector& trans) { @@ -665,26 +597,6 @@ double cMathUtil::Sigmoid(double x, double gamma, double bias) return val; } -int cMathUtil::SampleDiscreteProb(const Eigen::VectorXd& probs) -{ - assert(std::abs(probs.sum() - 1) < 0.00001); - double rand = RandDouble(); - - int rand_idx = gInvalidIdx; - int num_probs = static_cast(probs.size()); - for (int i = 0; i < num_probs; ++i) - { - double curr_prob = probs[i]; - rand -= curr_prob; - - if (rand <= 0) - { - rand_idx = i; - break; - } - } - return rand_idx; -} tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c) { @@ -791,32 +703,7 @@ bool cMathUtil::CheckNextInterval(double delta, double curr_val, double int_size return new_action; } -tVector cMathUtil::SampleRandPt(const tVector& bound_min, const tVector& bound_max) -{ - tVector pt = tVector(RandDouble(bound_min[0], bound_max[0]), - RandDouble(bound_min[1], bound_max[1]), - RandDouble(bound_min[2], bound_max[2]), 0); - return pt; -} -tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max) -{ - return SampleRandPtBias(bound_min, bound_max, 0.5 * (bound_max + bound_min)); -} - -tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus) -{ - double t = RandDouble(0, 1); - tVector size = bound_max - bound_min; - tVector new_min = focus + (t * 0.5) * size; - tVector new_max = focus - (t * 0.5) * size; - tVector offset = (bound_min - new_min).cwiseMax(0); - offset += (bound_max - new_max).cwiseMin(0); - new_min += offset; - new_max += offset; - - return SampleRandPt(new_min, new_max); -} void cMathUtil::QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h index 4e7413ed5..6a2bf695e 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h +++ b/examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h @@ -1,11 +1,10 @@ #pragma once -#include #include "Eigen/Dense" #include "Eigen/StdVector" #include "Eigen/Geometry" -#include "Rand.h" + #define _USE_MATH_DEFINES #include "math.h" @@ -47,20 +46,6 @@ public: static double NormalizeAngle(double theta); - // rand number - static double RandDouble(); - static double RandDouble(double min, double max); - static double RandDoubleNorm(double mean, double stdev); - static double RandDoubleExp(double lambda); - static double RandDoubleSeed(double seed); - static int RandInt(); - static int RandInt(int min, int max); - static int RandUint(); - static int RandUint(unsigned int min, unsigned int max); - static int RandIntExclude(int min, int max, int exc); - static void SeedRand(unsigned long int seed); - static int RandSign(); - static bool FlipCoin(double p = 0.5); static double SmoothStep(double t); // matrices @@ -117,7 +102,6 @@ public: static double Sigmoid(double x); static double Sigmoid(double x, double gamma, double bias); - static int SampleDiscreteProb(const Eigen::VectorXd& probs); static tVector CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c); static bool ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max); @@ -134,19 +118,14 @@ public: // check if curr_val and curr_val - delta belong to different intervals static bool CheckNextInterval(double delta, double curr_val, double int_size); - static tVector SampleRandPt(const tVector& bound_min, const tVector& bound_max); - // samples a bound within the given bounds with a benter towards the focus pt - static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max); - static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus); - + static void QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist); static tQuaternion ProjectQuat(const tQuaternion& q, const tVector& dir); static void ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x); private: - static cRand gRand; - + template static T SignAux(T val) { diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp b/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp deleted file mode 100644 index 599d2d0b7..000000000 --- a/examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "Rand.h" -#include -#include -#include - -cRand::cRand() -{ - unsigned long int seed = static_cast(time(NULL)); - mRandGen = std::default_random_engine(seed); - mRandDoubleDist = std::uniform_real_distribution(0, 1); - mRandDoubleDistNorm = std::normal_distribution(0, 1); - mRandIntDist = std::uniform_int_distribution(std::numeric_limits::min() + 1, std::numeric_limits::max()); // + 1 since there is one more neg int than pos int - mRandUintDist = std::uniform_int_distribution(std::numeric_limits::min(), std::numeric_limits::max()); -} - -cRand::cRand(unsigned long int seed) -{ - Seed(seed); -} - -cRand::~cRand() -{ -} - -double cRand::RandDouble() -{ - return mRandDoubleDist(mRandGen); -} - -double cRand::RandDouble(double min, double max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max] - double rand_double = mRandDoubleDist(mRandGen); - rand_double = min + (rand_double * (max - min)); - return rand_double; -} - -double cRand::RandDoubleExp(double lambda) -{ - std::exponential_distribution dist(lambda); - double rand_double = dist(mRandGen); - return rand_double; -} - -double cRand::RandDoubleNorm(double mean, double stdev) -{ - double rand_double = mRandDoubleDistNorm(mRandGen); - rand_double = mean + stdev * rand_double; - return rand_double; -} - -int cRand::RandInt() -{ - return mRandIntDist(mRandGen); -} - -int cRand::RandInt(int min, int max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max) - int delta = max - min; - int rand_int = std::abs(RandInt()); - rand_int = min + rand_int % delta; - - return rand_int; -} - -int cRand::RandUint() -{ - return mRandUintDist(mRandGen); -} - -int cRand::RandUint(unsigned int min, unsigned int max) -{ - if (min == max) - { - return min; - } - - // generate random double in [min, max) - int delta = max - min; - int rand_int = RandUint(); - rand_int = min + rand_int % delta; - - return rand_int; -} - -int cRand::RandIntExclude(int min, int max, int exc) -{ - int rand_int = 0; - if (exc < min || exc >= max) - { - rand_int = RandInt(min, max); - } - else - { - int new_max = max - 1; - if (new_max <= min) - { - rand_int = min; - } - else - { - rand_int = RandInt(min, new_max); - if (rand_int >= exc) - { - ++rand_int; - } - } - } - return rand_int; -} - -void cRand::Seed(unsigned long int seed) -{ - mRandGen.seed(seed); - mRandDoubleDist.reset(); - mRandDoubleDistNorm.reset(); - mRandIntDist.reset(); - mRandUintDist.reset(); -} - -int cRand::RandSign() -{ - return FlipCoin() ? -1 : 1; -} - -bool cRand::FlipCoin(double p) -{ - return (RandDouble(0, 1) < p); -} diff --git a/examples/SharedMemory/plugins/stablePDPlugin/Rand.h b/examples/SharedMemory/plugins/stablePDPlugin/Rand.h deleted file mode 100644 index c7dd543bf..000000000 --- a/examples/SharedMemory/plugins/stablePDPlugin/Rand.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include - -class cRand -{ -public: - cRand(); - cRand(unsigned long int seed); - virtual ~cRand(); - - virtual double RandDouble(); - virtual double RandDouble(double min, double max); - virtual double RandDoubleExp(double lambda); - virtual double RandDoubleNorm(double mean, double stdev); - virtual int RandInt(); - virtual int RandInt(int min, int max); - virtual int RandUint(); - virtual int RandUint(unsigned int min, unsigned int max); - virtual int RandIntExclude(int min, int max, int exc); - virtual void Seed(unsigned long int seed); - virtual int RandSign(); - virtual bool FlipCoin(double p = 0.5); - -private: - std::default_random_engine mRandGen; - std::uniform_real_distribution mRandDoubleDist; - std::normal_distribution mRandDoubleDistNorm; - std::uniform_int_distribution mRandIntDist; - std::uniform_int_distribution mRandUintDist; -}; \ No newline at end of file diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index d5ec6cb92..9694f4ddb 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -122,7 +122,7 @@ void btConvexPolyhedron::initialize() for (int p = 0; p < m_uniqueEdges.size(); p++) { - if (IsAlmostZero(m_uniqueEdges[p] - edge) || + if (IsAlmostZero1(m_uniqueEdges[p] - edge) || IsAlmostZero1(m_uniqueEdges[p] + edge)) { found = true;