I guess it is not that hard, but I have been stuck on that one for a while.
I have a joint that can rotate both direction. A sensor gives me the angle of the joint in the range -pi and +pi.
I would like to convert it in the range -infinity and +infinity. Meaning that if for example the joint rotate clockwise forever, the angle would start at 0 and then increase to infinity.
In matlab, the unwrap function does that very well:
newAngle = unwrap([previousAngle newAngle]);
previousAngle = newAngle;
Note: it is assumed the angle does not make big jump, nothing superior to PI for sure.
Note: I really looked hard before asking ...
Thanks !
The following function does the job, assuming the absolute difference between the input angles is less than 2*pi:
float unwrap(float previous_angle, float new_angle) {
float d = new_angle - previous_angle;
d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
return previous_angle + d;
}
If you need to unwrap an array, you can use this routine:
void unwrap_array(float *in, float *out, int len) {
out[0] = in[0];
for (int i = 1; i < len; i++) {
float d = in[i] - in[i-1];
d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
out[i] = out[i-1] + d;
}
}
After some work, came up with this. Seems to be working fine.
//Normalize to [-180,180):
inline double constrainAngle(double x){
x = fmod(x + M_PI,M_2PI);
if (x < 0)
x += M_2PI;
return x - M_PI;
}
// convert to [-360,360]
inline double angleConv(double angle){
return fmod(constrainAngle(angle),M_2PI);
}
inline double angleDiff(double a,double b){
double dif = fmod(b - a + M_PI,M_2PI);
if (dif < 0)
dif += M_2PI;
return dif - M_PI;
}
inline double unwrap(double previousAngle,double newAngle){
return previousAngle - angleDiff(newAngle,angleConv(previousAngle));
}
I used code from this post:
Dealing with Angle Wrap in c++ code
// wrap to [-pi,pi]
inline double angle_norm(double x)
{
x = fmod(x + M_PI, M_2PI);
if (x < 0)
x += M_2PI;
return x - M_PI;
}
double phase_unwrap(double prev, double now)
{
return prev + angle_norm(now - prev);
}
This works.
Related
I have a mathematical control problem which I solve through Backward induction. The mathematical problem is the following :
with K less than n.
And final conditions
What is J(0,0,0) ?
For this purpose I am using c++ and mingw 32 bit as a compiler.
The problem is the code (below) which solve the problem is an induction and does not provide any results if n,M > 15.
I have tried to launch n=M=100 for 4 days but no results.
Does anyone have a solution? Is it a compiler option to change (the processor memory is not enough)? The complexity is too big?
Here my code
const int n = 10;
const int M = 10;
double J_naive (double K, double Z, double W)
{
double J_tmp = exp(100.0);
double WGreaterThanZero = 0.0;
//Final condition : Boundaries
if (K == n)
{
if (W > 0) WGreaterThanZero = 1.0;
else WGreaterThanZero = 0.0;
if (Z >= WGreaterThanZero) return 0.0;
return exp(100.0);//Infinity
}
//Induction
else if (K < n)
{
double y;
for (int i = 0; i <= M; i++)
{
y = ((double) i)/M;
{
J_tmp = std::min (J_tmp, ((double) n)*y*y +
0.5*J_naive(K+1.0, Z+y, W + 1.0/sqrt(n)) +
0.5*J_naive(K+1.0, Z+y, W - 1.0/sqrt(n)) );
}
}
}
return J_tmp;
}
int main()
{
J_naive(0.0, 0.0, 0.0);
}
You can try the following, completely untested DP code. It needs around 24*n^3*M bytes of memory; if you have that much memory, it should run within a few seconds. If there is some value that will never appear as a true return value, you can get rid of seen_[][][] and use that value in result_[][][] to indicate that the subproblem has not yet been solved; this will reduce memory requirements by about a third. It's based on your code before you made edits to fix bugs.
const int n = 10;
const int M = 10;
bool seen_[n][n * M][2 * n]; // Initially all false
double result_[n][n * M][2 * n];
double J_naive(unsigned K, unsigned ZM, double W0, int Wdsqrtn)
{
double J_tmp = exp(100.0);
double WGreaterThanZero = 0.0;
double Z = (double) ZM / M;
double W = W0 + Wdsqrtn * 1./sqrt(n);
//Final condition : Boundaries
if (K == n)
{
if (W > 0) WGreaterThanZero = 1.0;
else WGreaterThanZero = 0.0;
if (Z >= WGreaterThanZero) return 0.0;
return exp(100.0);//Infinity
}
//Induction
else if (K < n)
{
if (!seen_[K][ZM][Wdsqrtn + n]) {
// Haven't seen this subproblem yet: compute the answer
for (int i = 0; i <= M; i++)
{
J_tmp = std::min (J_tmp, ((double) n)*i/M*i/M +
0.5*J_naive(K+1, ZM+i, W0, Wdsqrtn+1) +
0.5*J_naive(K+1, ZM+i, W0, Wdsqrtn-1) );
}
result_[K][ZM][Wdsqrtn + n] = J_tmp;
seen_[K][ZM][Wdsqrtn + n] = true;
}
}
return result_[K][ZM][Wdsqrtn + n];
}
I've recently been given a problem by my teacher about some mathematical equation / formula called the arctanx formula. The question is:
According to the Arctanx(x) = x - ((x ^ 3) / 3) + ((x ^ 5) / 5) - ((x ^
7) / 7) + ...and π = 6 * arctanx(1 / sqrt(3)), Create function arctanx(x)
, and find pi when the last "number"(like this ((x ^ y) / y)) is right before
and bigger than 10 ^ -6, or you can say that no "number" can be smaller than
that number without being smaller than 10 ^ -6.
I tried to code it out, but there is a bug in it.
# include<iostream>
# include<math.h>
using namespace std;
float arctanx() {
long double pi = 3.1415926535897;
int i = 0; // 0 = +, 1 = -
float sum = 0;
float lsum;
for (int y = 1; y < pi; y += 2) {
if (lsum > 0.000001) {
if (i == 0) {
lsum = pow(1 / sqrt(3), y) / y;
sum += pow(1 / sqrt(3), y) / y;
i++;
} else if (i == 1) {
lsum = pow(1 / sqrt(3), y) / y;
sum -= pow(1 / sqrt(3), y) / y;
i--;
}
} else {
break;
}
}
sum = sum * 6;
return sum;
}
int main() {
cout << arctanx();
return 0;
}
It should have a output of some number not equal to zero, but I got 0 from running this.
Your program has Undefined Behavior because you are using the uninitialized float lsum; in the comparison if (lsum > 0.000001).
What probably happens in your case is that lsum happens to be less than or equal to 0.000001 and your for immediately breaks without doing anything causing your function to return 0 * 6 which is obviously 0.
Create function arctanx(x)
The function defined in the posted code doesn't accept any parameter, it just uses the hardwired (and repeated) value 1 / sqrt(3) and tries to return an approximated value of π instead of the arctangent of x.
It also has undefined behavior, beeing lsum uninitialized (therefore having an indeterminate value) when it is first used in the comparison inside the loop.
Consider this implementation, but be advised that this particular polinomial expansion diverges for values of x greater than 1.
#include <iostream>
#include <iomanip>
#include <cmath>
double arctanx(double x);
int main()
{
double pi = 6.0 * arctanx(1.0 / std::sqrt(3));
std::cout << std::setprecision(8) << pi << '\n';
}
double arctanx(double x)
{
// You can take advantage of a running power, instad of calculating
// pow(x, i) at every iteration
double sq_x = x * x;
double pow_x = x * sq_x;
double err = 1e-6;
// Instead of keeping track of the alternating sign, you can use
// two separate partial sums
double sum_pos_term = x;
double sum_neg_term = 0.0;
for (int i = 3; i < 33; i += 2) // <- Limit the number of iterations
{
if (pow_x < err * i)
break;
sum_neg_term += pow_x / i;
i += 2;
pow_x *= sq_x;
if (pow_x < err * i)
break;
sum_pos_term += pow_x / i;
pow_x *= sq_x;
}
return sum_pos_term - sum_neg_term;
}
My question is that is it possible to convert a vector which stores samples of original CDF (cumulative density function)...
something like this:
class normal
{
public:
float mean;
float sigma;
float variance = sigma * sigma;
float left_margin = mean - 4 * sigma;
float right_margin = mean + 4 * sigma;
normal():mean(0), sigma(1){}
normal(float m, float s):mean(m), sigma(s){}
float cdf(float x);
float pdf(float x);
};
float normal::pdf(float x)
{
if (x < left_margin || x > right_margin) return 0;
float coefficient = 1 / (float)sqrt(2 * PI * variance);
float x_mean = x - mean;
float result = coefficient * exp(-(x_mean * x_mean) / 2 * variance);
return result;
}
float normal::cdf(float x)
{
if (x <= left_margin) return 0;
if (x >= right_margin) return 1;
float x_mean = x - mean;
float result = (float)(0.5 * (1 + erf((x_mean) / sqrt(2 * variance))));
if (result > 1) return 1;
else return result;
}
std::vector<float> discrete_normal_cdf(normal& X)
{
std::vector<float> vec;
float L = (float)(X.left_margin);
float R = (float)(1.2 * X.right_margin);
while (L <= R)
{
vec.push_back(X.cdf(L));
L = (float)(L + 0.1);
}
std::vector<float> tmp;
// take three samples
tmp.push_back(vec.at(1)); // first non_zero element
tmp.push_back(vec.at(40)); // add element with value of 0.5
tmp.push_back(vec.at(80)); // element with value of 0.99
std::vector<float> cdf_v(5, 0);
for (auto i = 0; i < tmp.size(); i++)
cdf_v.push_back(tmp.at(i));
int l = 0;
while (l < 5)
{
cdf_v.push_back(1);
l++;
}
return cdf_v;
}
In fact what I need is this: if we have a normal
normal n1(5, 1);
take samples of its CDF to piece wise linear CDF:
vector<float> foo = discrete_normal_cdf(n1);
then reconstruct the piecewise linear CDF into normal
normal function(foo)
{
return normal(5, 1);
}
Is this function valid?
I wrote a function which takes a vector as an input
and search all the elements of the vector the for the value of 0.5
and returns the index of that element as the mean of the normal
but it not always true.
normal vec2normal(vector<float>& vec)
{
int mean;
mean = std::find(vec.begin(), vec.end(), 0.5) - vec.begin();
return normal(mean, 1);
}
I have no idea how to do this, so any suggestions will be appreciated
thank you.
Im studying some code and I would like help with some math. Im trying to solve the equation of the tangent line on a circle with given point of tangency.
//(x1 - p)(x - p) +(y1 - q)(y - q) = r^2 I understand this formula
//variables
//x1 = point.x
//y1 = point.y
//p = center.x
//q = center.y
//r = radius
edit: here is the whole function, maybe it will help. My teacher gave it to me to study, but maybe he is trolling me :D
const std::pair<double, double> Arc::tangentEquation(const glm::vec3& center, const glm::vec3& pointA, float radius) const {
if (radius <= 0.0f)
throw std::domain_error("Radius can't be negative or 0");
// Jednadžba tangente u točki T
// (x1 - p)(x - p) + (y1 - q)(y - q) = r^2
glm::vec3 point = pointA + center;
double px = -1 * (center.x * point.x);
double qy = -1 * (center.y * point.y);
double x = point.x - center.x;
double y = point.y - center.y;
double k = 0.0;
double l = (pow(radius, 2) - (px + pow(center.x, 2) + qy + pow(center.y, 2)));
if (y == 0) { // paralelan s x os
k = l / x;
l = 0;
} else if (x == 0) { // paralelan s y os
l = l / y;
k = 0;
} else {
k = -x / y;
l = l / y;
}
return std::pair<double, double>(k, l);
}
The code does not implement the formula on the first line, so I don't think it is strange that you don't understand :-)
(x1 - p)(x - p) + (y1 - q)(y - q)
If we write out all the terms in the parenthesis multiplication, we get:
x1*x - p*x - p*x1 + p^2 + y1*y - q*y - q*y1 + q^2
(https://www.youtube.com/watch?v=3s_lroR5_1U for very pedagogic explanation)
But your code looses half of these terms....?
I tried a quick and dirty translation of the code here.
However, my version outputs noise comparable to grey t-shirt material, or heather if it please you:
#include <fstream>
#include "perlin.h"
double Perlin::cos_Interp(double a, double b, double x)
{
ft = x * 3.1415927;
f = (1 - cos(ft)) * .5;
return a * (1 - f) + b * f;
}
double Perlin::noise_2D(double x, double y)
{
/*
int n = (int)x + (int)y * 57;
n = (n << 13) ^ n;
int nn = (n * (n * n * 60493 + 19990303) + 1376312589) & 0x7fffffff;
return 1.0 - ((double)nn / 1073741824.0);
*/
int n = (int)x + (int)y * 57;
n = (n<<13) ^ n;
return ( 1.0 - ( (n * (n * n * 15731 + 789221) + 1376312589) & 0x7fffffff) / 1073741824.0);
}
double Perlin::smooth_2D(double x, double y)
{
corners = ( noise_2D(x - 1, y - 1) + noise_2D(x + 1, y - 1) + noise_2D(x - 1, y + 1) + noise_2D(x + 1, y + 1) ) / 16;
sides = ( noise_2D(x - 1, y) + noise_2D(x + 1, y) + noise_2D(x, y - 1) + noise_2D(x, y + 1) ) / 8;
center = noise_2D(x, y) / 4;
return corners + sides + center;
}
double Perlin::interp(double x, double y)
{
int x_i = int(x);
double x_left = x - x_i;
int y_i = int(y);
double y_left = y - y_i;
double v1 = smooth_2D(x_i, y_i);
double v2 = smooth_2D(x_i + 1, y_i);
double v3 = smooth_2D(x_i, y_i + 1);
double v4 = smooth_2D(x_i + 1, y_i + 1);
double i1 = cos_Interp(v1, v2, x_left);
double i2 = cos_Interp(v3, v4, x_left);
return cos_Interp(i1, i2, y_left);
}
double Perlin::perlin_2D(double x, double y)
{
double total = 0;
double p = .25;
int n = 1;
for(int i = 0; i < n; ++i)
{
double freq = pow(2, i);
double amp = pow(p, i);
total = total + interp(x * freq, y * freq) * amp;
}
return total;
}
int main()
{
Perlin perl;
ofstream ofs("./noise2D.ppm", ios_base::binary);
ofs << "P6\n" << 512 << " " << 512 << "\n255\n";
for(int i = 0; i < 512; ++i)
{
for(int j = 0; j < 512; ++j)
{
double n = perl.perlin_2D(i, j);
n = floor((n + 1.0) / 2.0 * 255);
unsigned char c = n;
ofs << c << c << c;
}
}
ofs.close();
return 0;
}
I don't believe that I strayed too far from the aforementioned site's directions aside from adding in the ppm image generation code, but then again I'll admit to not fully grasping what is going on in the code.
As you'll see by the commented section, I tried two (similar) ways of generating pseudorandom numbers for noise. I also tried different ways of scaling the numbers returned by perlin_2D to RGB color values. These two ways of editing the code have just yielded different looking t-shirt material. So, I'm forced to believe that there's something bigger going on that I am unable to recognize.
Also, I'm compiling with g++ and the c++11 standard.
EDIT: Here's an example: http://imgur.com/Sh17QjK
To convert a double in the range of [-1.0, 1.0] to an integer in range [0, 255]:
n = floor((n + 1.0) / 2.0 * 255.99);
To write it as a binary value to the PPM file:
ofstream ofs("./noise2D.ppm", ios_base::binary);
...
unsigned char c = n;
ofs << c << c << c;
Is this a direct copy of your code? You assigned an integer to what should be the Y fractional value - it's a typo and it will throw the entire noise algorithm off if you don't fix:
double Perlin::interp(double x, double y)
{
int x_i = int(x);
double x_left = x - x_i;
int y_i = int(y);
double y_left = y = y_i; //This Should have a minus, not an "=" like the line above
.....
}
My guess is if you're successfully generating the bitmap with the proper color computation, you're getting vertical bars or something along those lines?
You also need to remember that the Perlin generator usually spits out numbers in the range of -1 to 1 and you need to multiply the resultant value as such:
value * 127 + 128 = {R, G, B}
to get a good grayscale image.