get the distance using mpu 6050 accelemetor - mpu6050

I want to calculate traveled distance of mpu 6050 sensor using the acceleration of x,y,z. How to get the traveled distance that sensor traveled using accelerometer values by integrating the accelerometer values.Can you please explain the code example for getting the velocity and displacement
a_present = aaReal.x * 9.8;
a_average = (a_present + a_previous) / 2;
v_present = v_previous + a_average * 0.001 ;
v_average = (v_previous + v_present) / 2;
s_present = v_previous * 0.001 + 0.5 * a_average * 0.001 * 0.001;
Serial.println("v_present " + (String)v_present + "\t\ts_present " +(String)s_present+"\t\ts_all "+(String)s_all + "\ta_real "+(String)a_present);
a_previous = a_average;
v_previous = v_present;
s_all = s_all + s_present;

Related

Find minimum velocity needed to reach a given coordinate?

I am coding a simulation of ball movement. I have an updateBall function which runs every 100 miliseconds to update the location of the ball.
How could i find out the minimum velocity needed to reach a given target coordinate?
Below is the relevant code,
ball.x = 0;
ball.y = 0;
targetX = 100;
targetY = 200;
friction = 0.03;
dx = targetX - ball.x;
dy = targetY - ball.y;
distance = sqrt(dx*dx + dy*dy);
velocity = ?;
// runs every 100ms
updateBall()
{
ball.x += velocity;
ball.y += velocity;
velocity -= friction;
}
It seems wrong that you apply friction to both components separately - in this case ball can stop vertically but move horizontally - looks strange, doesn't it?
Is is worth to apply acceleration to velocity vector. Seems you have straight moving - so you can precalculate coeffients for both components.
Concerning needed velocity:
distance = sqrt(dx*dx + dy*dy)
v_final = v0 - a * t = 0 so t = v0 / a
distance = v0 * t - a * t^2 / 2 substitute t and get
distance = v0^2 / (2a)
and finally initial velocity to provide moving at distance
v0 = sqrt(2*distance*a)
where a is acceleration proportional to your friction accordingly to elementary interval dt (100 ms ).
friction = a * dt
a = friction / dt
v0.x = v0 * dx / distance = v0 * coefX
v0.y = v0 * dy / distance = v0 * coefY
at every stage you update v value and get components
v = v - friction
v.x = v * coefX
v.y = v * coefY

Intersection between two vectors

Is there efficient way to find intersection between two vectors?
Ray is infinite so one way is to turn one vector to ray, find intersection between that ray and other vector. Then if there is intersection, find distance between intersection and first vector.
If it is 0 then there is intersection between two vectors, if i don't miss something.
But is there better way in three.js?
That is more a math question than a three.js one, as you mentioned you could use existing methods on the Ray object and work your way out from there, but I doubt this would be optimal as you are concerned about performance.
The right approach is probably to determine the shortest distance between two segments and if this distance is zero then they intersect. You can find a similar discussion on that thread: The Algorithm to Find the Point of Intersection of Two 3D Line Segment
And here I found a C algorithm that does exactly that, it should translate to Javascript without much efforts:
typedef struct {
double x,y,z;
} XYZ;
/*
Calculate the line segment PaPb that is the shortest route between
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Return FALSE if no solution exists.
*/
int LineLineIntersect(
XYZ p1,XYZ p2,XYZ p3,XYZ p4,XYZ *pa,XYZ *pb,
double *mua, double *mub)
{
XYZ p13,p43,p21;
double d1343,d4321,d1321,d4343,d2121;
double numer,denom;
p13.x = p1.x - p3.x;
p13.y = p1.y - p3.y;
p13.z = p1.z - p3.z;
p43.x = p4.x - p3.x;
p43.y = p4.y - p3.y;
p43.z = p4.z - p3.z;
if (ABS(p43.x) < EPS && ABS(p43.y) < EPS && ABS(p43.z) < EPS)
return(FALSE);
p21.x = p2.x - p1.x;
p21.y = p2.y - p1.y;
p21.z = p2.z - p1.z;
if (ABS(p21.x) < EPS && ABS(p21.y) < EPS && ABS(p21.z) < EPS)
return(FALSE);
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
denom = d2121 * d4343 - d4321 * d4321;
if (ABS(denom) < EPS)
return(FALSE);
numer = d1343 * d4321 - d1321 * d4343;
*mua = numer / denom;
*mub = (d1343 + d4321 * (*mua)) / d4343;
pa->x = p1.x + *mua * p21.x;
pa->y = p1.y + *mua * p21.y;
pa->z = p1.z + *mua * p21.z;
pb->x = p3.x + *mub * p43.x;
pb->y = p3.y + *mub * p43.y;
pb->z = p3.z + *mub * p43.z;
return(TRUE);
}

Why same OpenModelica code behaves differently in two files?

I'm trying to model the trajectory of projectile in modelica, but when I model air_pressure dependency on the height I get weird result, now if I simply copy only the air_pressure's code to another file, I get pretty fine output. Why's that so?
This is the original code for projectile,
class proj
//position
Real x(start = 6371000);
Real y(start = 0);
Real z(start = 0);
Real r(start = 6371000);
Real row(start = 6371000);
Real h(start = 0);
//velocity
Real v_x(start = 2000);
Real v_y(start = 0);
Real v_z(start = 0);
Real v_mag(start = sqrt(4000000));
Real v_row(start = sqrt(4000000));
//acceleration
Real a_x();
Real a_y();
Real a_z();
Real a_mag();
//total force
Real ft_x();
Real ft_y();
Real ft_z();
Real ft_mag();
//Gravitational force
Real fg_x();
Real fg_y();
Real fg_z();
Real fg_mag();
//Drag force
Real fd_x();
Real fd_y();
Real fd_z();
Real fd_mag();
Real energy();
//Fictitious force
Real ff_x();
Real ff_y();
Real ff_z();
Real ff_mag();
//gravitation variables
Real g(start = G * earth_mass / earth_radius ^ 2);
//Drag variables
Real air_density(start = surface_density);
Real air_pressure(start = surface_pressure);
//_________________________________________________________________________
//Earth parameters
parameter Real earth_radius = 6371000;
parameter Real earth_mass = 5.972 * 10 ^ 24;
parameter Real earth_rot_vel = 7.292115 * 10 ^ (-5);
//Missile parameters
parameter Real missile_effective_area = 0.785398;
parameter Real missile_mass = 200;
parameter Real Cd = 0.02;
//Air parameters
parameter Real temp_lapse_rate = 0.0065;
parameter Real air_molar_mass = 28.97 / 1000;
parameter Real surface_temp = 303.15;
parameter Real surface_pressure = 101325;
parameter Real surface_density = 1.1644;
//constants
parameter Real pi = 3.14159265358979;
parameter Real G = 6.67384 * 10 ^ (-11);
parameter Real R = 8.3144621;
//__________________________________________________________________________
equation
//position
der(x) = if h < 0 then 0 else v_x;
der(y) = if h < 0 then 0 else v_y;
der(z) = if h < 0 then 0 else v_z;
r = sqrt(x ^ 2 + y ^ 2 + z ^ 2);
row = sqrt(x ^ 2 + y ^ 2);
h = r - earth_radius;
//velocity
der(v_x) = a_x;
der(v_y) = a_y;
der(v_z) = a_z;
v_mag = sqrt(v_x ^ 2 + v_y ^ 2 + v_z ^ 2);
v_row = sqrt(v_x ^ 2 + v_y ^ 2);
//acceleration
a_x = ft_x / missile_mass;
a_y = ft_y / missile_mass;
a_z = ft_z / missile_mass;
a_mag = ft_mag / missile_mass;
//total force
ft_x = fg_x + fd_x + ff_x;
ft_y = fg_y + fd_y + ff_y;
ft_z = fg_z + fd_z + ff_z;
ft_mag = sqrt(ft_x ^ 2 + ft_y ^ 2 + ft_z ^ 2);
//Gravitational force
fg_x = -1 * fg_mag * row / r * x / r;
fg_y = -1 * fg_mag * row / r * y / r;
fg_z = -1 * fg_mag * z / r;
fg_mag = G * earth_mass * missile_mass / r ^ 2;
//Drag force
fd_x = if v_mag > 0 then -1 * fd_mag * v_row / v_mag * v_x / v_mag else 0;
fd_y = if v_mag > 0 then -1 * fd_mag * v_row / v_mag * v_y / v_mag else 0;
fd_z = if v_mag > 0 then -1 * fd_mag * v_z / v_mag else 0;
fd_mag = 0.5 * air_density * v_mag ^ 2 * Cd * missile_effective_area;
//Fictitious force
ff_x = missile_mass * earth_rot_vel * (2 * v_y + earth_rot_vel * x);
ff_y = missile_mass * earth_rot_vel * (-2 * v_x + earth_rot_vel * y);
ff_z = 0;
ff_mag = sqrt(ff_x ^ 2 + ff_y ^ 2 + ff_z ^ 2);
energy = 0.5 * missile_mass * v_mag * v_mag - G * earth_mass * missile_mass / r;
//Gravitation variables
g = fg_mag / missile_mass;
//Drag variables
air_density = air_pressure * air_molar_mass / (R * surface_temp);
air_pressure = if h > 46600 then 0 else surface_pressure * (1 - temp_lapse_rate * (r - earth_radius) / surface_temp) ^ (g * air_molar_mass / (R * temp_lapse_rate));
end proj;
This the extracted air_pressure's code,, that works fine alone but behaves weirdly with whole projectile code,
class check
Real g;
Real h(start = 0);
Real pressure;
//Earth parameters
parameter Real earth_radius = 6371000;
parameter Real earth_mass = 5.972 * 10 ^ 24;
//Air parameters
parameter Real temp_lapse_rate = 0.0065;
parameter Real air_molar_mass = 28.97 / 1000;
parameter Real surface_temp = 303.15;
parameter Real surface_pressure = 101325;
//constants
parameter Real pi = 3.14159265358979;
parameter Real G = 6.67384 * 10 ^ (-11);
parameter Real R = 8.3144621;
equation
g = G * earth_mass / (earth_radius + h) ^ 2;
h = time;
pressure = if h > 46600 then 0 else surface_pressure * (1 - temp_lapse_rate * h / surface_temp) ^ (g * air_molar_mass / (R * temp_lapse_rate));
end check;
This is the air_pressure graph vs time from the class proj
This the air_pressure graph vs time(=height) from class check containing only air_pressure's code
You should check the value of h in your system. I suspect something strange is happening with h.
Its not clear what x refers to. From the start value, x seems to refer to the distance from the center of the earth and if this is the case then what is r? I am assuming x,y & z refer to the position of the projectile in 3D space w.r.t a point of reference on the ground (surface of the earth) and if this correct, then I would suggest defining x, y in terms of latitude and longitude and z as the height from the surface of the earth.
This should fix the ambiguity in h.

Quadratic Bézier Curve: Calculate Points

I'd like to calculate a point on a quadratic curve. To use it with the canvas element of HTML5.
When I use the quadraticCurveTo() function in JavaScript, I have a source point, a target point and a control point.
How can I calculate a point on the created quadratic curve at let's say t=0.5 with "only" knowing this three points?
Use the quadratic Bézier formula, found, for instance, on the Wikipedia page for Bézier Curves:
In pseudo-code, that's
t = 0.5; // given example value
x = (1 - t) * (1 - t) * p[0].x + 2 * (1 - t) * t * p[1].x + t * t * p[2].x;
y = (1 - t) * (1 - t) * p[0].y + 2 * (1 - t) * t * p[1].y + t * t * p[2].y;
p[0] is the start point, p[1] is the control point, and p[2] is the end point. t is the parameter, which goes from 0 to 1.
In case somebody needs the cubic form:
//B(t) = (1-t)**3 p0 + 3(1 - t)**2 t P1 + 3(1-t)t**2 P2 + t**3 P3
x = (1-t)*(1-t)*(1-t)*p0x + 3*(1-t)*(1-t)*t*p1x + 3*(1-t)*t*t*p2x + t*t*t*p3x;
y = (1-t)*(1-t)*(1-t)*p0y + 3*(1-t)*(1-t)*t*p1y + 3*(1-t)*t*t*p2y + t*t*t*p3y;
I created this demo :
// x = a * (1-t)³ + b * 3 * (1-t)²t + c * 3 * (1-t)t² + d * t³
//------------------------------------------------------------
// x = a - 3at + 3at² - at³
// + 3bt - 6bt² + 3bt³
// + 3ct² - 3ct³
// + dt³
//--------------------------------
// x = - at³ + 3bt³ - 3ct³ + dt³
// + 3at² - 6bt² + 3ct²
// - 3at + 3bt
// + a
//--------------------------------
// 0 = t³ (-a+3b-3c+d) + => A
// t² (3a-6b+3c) + => B
// t (-3a+3b) + => c
// a - x => D
//--------------------------------
var A = d - 3*c + 3*b - a,
B = 3*c - 6*b + 3*a,
C = 3*b - 3*a,
D = a-x;
// So we need to solve At³ + Bt² + Ct + D = 0
Full example here
may help someone.
I edited talkhabis answer (cubic curve) so the curve is displayed with the right coordinates. (Couldn't comment)
The Y-coordinates needed to be changed (-p[].y+150). (A new variable for that might be a nicer and more efficient solution, but you get the idea)
// Apply points to SVG and create the curve and controllers :
var path = document.getElementById('path'),
ctrl1 = document.getElementById('ctrl1'),
ctrl2 = document.getElementById('ctrl2'),
D = 'M ' + p0.x + ' ' + (-p0.y+150) +
'C ' + c0.x + ' ' + (-c0.y+150) +', ' + c1.x + ' ' + (-c1.y+150) + ', ' + p1.x + ' ' + (-p1.y+150);
path.setAttribute('d',D);
ctrl1.setAttribute('d','M'+p0.x+','+(-p0.y+150)+'L'+c0.x+','+(-c0.y+150));
ctrl2.setAttribute('d','M'+p1.x+','+(-p1.y+150)+'L'+c1.x+','+(-c1.y+150));
// Lets test the "Bezier Function"
var t = 0, point = document.getElementById('point');
setInterval(function(){
var p = Bezier(p0,c0,c1,p1,t);
point.setAttribute('cx',p.x);
point.setAttribute('cy',-p.y+150);
t += 0.01;
if(t>=1) t=0;
},50);
// OK ... Now tring to get "y" on cruve based on mouse "x" :
var svg = document.getElementById('svg'),
point2 = document.getElementById('point2');
svg.onmousemove = function(e){
var x = (e.pageX - 50)/2,
y = (e.pageY - 50)/2;
// "-50" because of "50px margin" on the left side
// and "/2" because the svg width is 300 units and 600 px => 300 = 600/2
// Get the x,y by mouse x
var p = YBX(p0,c0,c1,p1,x);
point2.setAttribute('cx',p.x);
point2.setAttribute('cy',-p.y+150);
}
http://jsfiddle.net/u214gco8/1/
I also created some C-Code to test the results for the cubic curve. Just enter the X and Y coordinates in the main function.
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
void bezierCurve(int x[] , int y[])
{
double xu = 0.0 , yu = 0.0 , u = 0.0 ;
int i = 0 ;
for(u = 0.0 ; u <= 1.0 ; u += 0.05)
{
xu = pow(1-u,3)*x[0]+3*u*pow(1-u,2)*x[1]+3*pow(u,2)*(1-u)*x[2]
+pow(u,3)*x[3];
yu = pow(1-u,3)*y[0]+3*u*pow(1-u,2)*y[1]+3*pow(u,2)*(1-u)*y[2]
+pow(u,3)*y[3];
printf("X: %i Y: %i \n" , (int)xu , (int)yu) ;
}
}
int main(void) {
int x[] = {0,75,50,300};
int y[] = {0,2,140,100};
bezierCurve(x,y);
return 0;
}
https://ideone.com/glLXcB
Just a note: If you are using the usual formulas presented here then don't expect t = 0.5 to return the point at half of the curve's length.. In most cases it won't.
More on this here under "§23 — Tracing a curve at fixed distance intervals" and here.

Circle collision response

I'm working on an Android game and I need to bounce 2 circles of each other (like 2 pool balls bouncing off each other). The collision is an elastic collision, and I need to calculate only 1 circles (called a Particle in my code) new velocity after the collision (the other circle, called a Barrier in my code will remain stationary and will not move because of a collision).
I am using a formula that I found on Wikipedia (http://en.wikipedia.org/wiki/Elastic_collision), but my end result for the new velocity of the particle after the collision is exactly the same as the velocity before the collision?
This is def wrong but I cant see where I am going wrong. Can anyone spot where I am going wrong?
I have just used a Java program to simulate my velocities and locations for the 2 circles as I dont wanna try it in my main Android game at the moment for fear of "breaking something"
Here is what I have so far (like I mentioned, this is just a simulation in NetBeans for the moment and I will use a menthod in my Android game to keep things a bit tidier):
double randomNextDouble = (new Random()).nextDouble();
System.out.println("Random.nextDouble: " + randomNextDouble);
double mathPI = Math.PI * 2;
System.out.println("Math PI: " + mathPI);
// get a random direction for the Particle to move towards
double direction = (new Random()).nextDouble() * Math.PI * 2;
System.out.println("Direction: " + direction);
// Then set the Particle's velocity - Increase to make Particles move faster
int velocity = 10;
System.out.println("Velocity: " + velocity);
// Then calculate the xv and the yv
// Velocity value on the x and y axis
double xv = (velocity * Math.cos(direction));
double yv = (velocity * Math.sin(direction));
System.out.println("\nXV: " + xv + "\nYV: " + yv);
// Genareting a random number for the Particle and Barrier's positions on screen
double Xmin = 0;
double Xmax = 300;
double Ymin = 0;
double Ymax = 300;
double randomNumber1 = Xmin + (int)(Math.random() * ((Xmax - Xmin) + 1));
double randomNumber2 = Ymin + (int)(Math.random() * ((Ymax - Ymin) + 1));
double randomNumber3 = Xmin + (int)(Math.random() * ((Xmax - Xmin) + 1));
double randomNumber4 = Ymin + (int)(Math.random() * ((Ymax - Ymin) + 1));
// Setting the Particle and Barrier's radius
double particleRadius = 8;
double barrierRadius = 16;
// Setting up the Particle and Barrier's mass
double particleMass = 100;
double barrierMass = 200;
// Assigning a random number to the Particle to simulate its position on screen
double particleX = randomNumber1;
double particleY = randomNumber2;
System.out.println("\nParticle X: " + particleX + " Particle Y: " + particleY);
// Assigning a random number to the Barrier to simulate its position on screen
double barrierX = randomNumber3;
double barrierY = randomNumber4;
System.out.println("Barrier X: " + barrierX + " Barrier Y: " + barrierY);
double distanceXToBarrier = barrierX - particleX;
System.out.println("\nBarrier X - Particle X: " + distanceXToBarrier);
double distanceYToBarrier = barrierY - particleY;
System.out.println("Barrier Y - Particle Y: " + distanceYToBarrier);
// Get the distance between the Particle and the Barrier
// Used for collision detection
double distance = Math.sqrt((distanceXToBarrier * distanceXToBarrier) + (distanceYToBarrier * distanceYToBarrier));
System.out.println("\nDistance: " + distance);
// Check to see if the Particle and Barrier has collided
if (distance <= particleRadius + barrierRadius)
{
System.out.println("Distance is less than 2 Radii");
}
else
System.out.println("Distance is NOT less than 2 Radii");
// Velx = (v1.u) * u + (v1 - (v1.u) * u)
// Vely = (v1.u) * u + (v1 - (v1.u) * u)
// Where v1 = xv and yv respectively
// Break it into 2 equations
// (v1.u) * u AND
// (v1 - (v1.u) * u)
//
// u = normalised Vector
// To normalize you just devide the x, y, z coords by the length of the vector.
// This then gives you the Unit Vector.
//
//Normalize the vector
double particleXNormalized = particleX * (1.0 / distance);
double particleYNormalized = particleY * (1.0 / distance);
System.out.println("\nParticle X Normalised: " + particleXNormalized +
"\nParticle Y Normalised: " + particleYNormalized);
// Calculating the first part of the eqaution
// (v1.u)
double v1DotUForX = xv * particleXNormalized;
double v1DotUForY = yv * particleYNormalized;
System.out.println("\nv1.u for X: " + v1DotUForX +
"\nv1.u for Y: " + v1DotUForY);
// The first part of the equation
// (v1.u) * u
double part1X = v1DotUForX * particleXNormalized;
double part1Y = v1DotUForY * particleYNormalized;
System.out.println("\nPart 1 for X: " + part1X +
"\nPart 1 for Y: " + part1Y);
// The second part of the equation
// (v1 - (v1.u) * u)
double part2X = (xv - (v1DotUForX) * particleXNormalized);
double part2Y = (yv - (v1DotUForY) * particleYNormalized);
System.out.println("\nPart 2 for X: " + part2X +
"\nPart 2 for Y: " + part2Y);
// Solving for:
// (((mass 1 - mass2) / (mass1 + mass2) * (v1.u) * u + ((2mass2) / (mass1 + mass2) * ((v1.u) * u))) +
// (v1 - (v1.u) * u))
double newXV = ((((particleMass - barrierMass) / (particleMass + barrierMass)) * part1X) + (((2 * barrierMass) / (particleMass + barrierMass)) * part1X) + part2X);
double newYV = ((((particleMass - barrierMass) / (particleMass + barrierMass)) * part1Y) + (((2 * barrierMass) / (particleMass + barrierMass)) * part1Y) + part2Y);
System.out.println("\nNew XV: " + newXV + "\nNew YV: " + newYV);
Looking at your algorithm, you appear to have made errors in the implementation. Why are you normalizing the coordinates of the particle? Shouldn't you be doing that to the velocity? In the usual equations, u is velocity, not position.
And why do you give the particle a random velocity (xv, yv) that has nothing to do with the two random coordinates you set up for the particle and barrier? (Surely the velocity should be some multiple of (barrier - particle) vector?)

Resources