I am trying to simulate the collision of two spheres on Fortran 90 by using the equations of classical mechanics.
Firstly, I'm doing this on one dimension: I am fixing a sphere on the ground and putting the other one at a certain height (both are at rest at t=0), and both of them have the same radius and mass.
Being so, the forces acting at the moving particle is the gravitational force and the elastic force (when the spheres start to collide). So, at first, the moving sphere is only subjected to gravitational force, that is, Fg = -g.m. I'm considering only one dimension, so since the particle falling will give me lower values than the height it was in the beginning, it is negative. Now, when the moving particle hits the fixed one, the latter will cause a force of Fe = -k.dx.n, where k is the elastic constant, dx is the overlap and n is the direction of that force (its norm is 1). The elastic force is restitutive, so it should be negative also. To be clear, dx = r1 + r2 - |x1 - x2| and n = (x1-x2)/|x1 -x2|, where r1, r2 are the radii and x1, x2 are the positions of the particles at given time.
The code compiles ok, the only problem is when I go check the positions for the moving particle - sometimes, it keeps getting lower (as it should at first) and then it continues to decrease to negative values - something that should not happen. Or, when the moving sphere does bounce back where it should, it goes back to a height HIGHER than the initial condition (it bounces higher, violating conservation of energy).
The thing is that, the best situation that it seems to be working (that last one, in which it bounces higher), the formula for the elastic force is actually not right (I put dx = r1 + r2 - |x1 + x2|, instead of a minus sign). I've tried correcting the higher bounce by changing some parameters (lowering and increasing elastic constant and lowering the time step), but with no success. Also, the higher bouncing I mentioned is about four times higher, so it is not some problem of error of calculations.
Here are the important parts of the code: where the forces are calculated, and the time integration:
coef1 = abs(1.0d0*x1 - 1.0d0*x2)
coef2 = (1.0d0*x1 - 1.0d0*x2)/coef1
Fx_elasticaparcial(m) = -1.0d0*k*((1.0d0*a1+1.0d0*a2) + 1.0d0*coef1)*coef2
Fx_elasticatotal(l) = 1.0d0*Fx_elasticatotal(l) +1.0d0*Fx_elasticaparcial(m)
forcax(l) = 1.0d0*flagx(l)*(1.0d0*gravidade(massadummy)+1.0d0*Fx_elasticatotal(l))
m is the partial elastic force and l is the total elastic force exerted ON a particle (I've done it thinking of adding more particles further down the road)
function gravidade(massa)
use parametros, only: g
implicit none
real*8 :: gravidade, massa
gravidade = -1.0d0*g*massa
end function gravidade
This one is a function for gravitational force
subroutine integracaoEuler (xo, xn, vxo, vxn, fx, mass)
use parametros, only: N, dt
implicit none
real*8 :: xo, xn, vxo, vxn, fx, mass
xn = 1.0d0*xo + 1.0d0*dt*vxo
xo = 1.0d0*xn
vxn = 1.0d0*vxo + 1.0d0*dt*((1.0d0*fx)/(1.0d0*mass))
vxo = 1.0d0*vxn
return
end subroutine integracaoEuler
Euler integration subroutine
do while ((t + dt) < (tmax)) !time loop
t = t+dt
do i = 1, N !particle loop
do j = 1, N !neighbours of i particle loop
if (i .NE. j) then
if (abs(SQRT(1.0d0*xold(i) - 1.0d0*xold(j))**2) .LE. 1.0d0*a(i) + 1.0d0*a(j)) then
call forcastotais (i, j, m(i), a(i), a(j), xold(i), xold(j))
else
forcax(i) = 1.0d0*flagx(i)*gravidade(m(i))
end if
end if
end do
call integracaoEuler (xold(i), xnew(i), vxold(i), vxnew(i), forcax(i), m(i))
write(i, *) t, xold(i)
end do
end do
Main loop of the program
Sorry for making this really big, to fix it probably must be something easy. Again, I need to really use the equations of classical mechanics for this, not by putting the velocity to change direction when it hits the other sphere or that sort of stuff. I am frustrated with this, and don't know how to fix it (I have been stuck on this for 2 weeks). I've done everything else on the model I'm making (viscosity, cell-link) with a lot of particles, but this problem keeps reappearing and messing up my graphs. Any help is welcome; please make it explicit, since I am new to programming. Thank you in advance.
Related
Program Foucault
IMPLICIT NONE
REAL,DIMENSION(:),ALLOCATABLE :: t, x,y
REAL,PARAMETER :: pi=3.14159265358979323846, g=9.81
REAL :: L, vitessea, lat, h, omega, beta
INTEGER :: i , zeta
zeta=1000
Allocate(x(zeta),y(zeta),t(zeta))
L=67.
lat=49/180*pi
omega=sqrt(g/L)
h=0.01
Do i= 1,zeta
IF(i==1 .OR. i==2) THEN
t(1)=0.0
t(2)=0.0
x(1)=0.1
x(2)=1
y(1)=0.0
y(2)=0.0
ELSE
t(i+1)=real(i)*h
x(i+1)=(-omega**2*x(i)+2.0*((y(i)-y(i-1))/h)*latang(lat))*h**2+2.0*x(i)-x(i-1)
y(i+1)=(-omega**2*y(i)-2.0*((x(i)-x(i-1))/h)*latang(lat))*h**2+2.0*y(i)-y(i-1)
END IF
WRITE(40,*) t(i), x(i)
WRITE(60,*) t(i), y(i)
WRITE(50,*) x(i), y(i)
END DO
Contains
REAL Function latang(alpha)
REAL, INTENT(IN) :: alpha
REAL :: sol
latang=2*pi*sin(alpha)/86400
END FUNCTION
End Program Foucault
I'm trying to code the original Foucault Pendulum in Paris. My code seems to be working but so far, I could only get the below right graphic, "the flower" evolution. Therefore, I changed my parameters constantly to get the left graphic but I couldn't.
I took parameters of Foucault Pendulum installed in Paris with L=67, angular velocity of earth =2*pi/86400 and latitude of 49/180*pi.
My initial conditions are as written in the code. I tried a way range of parameters varying all of my initial conditions, my latitude and angular velocity but i couldn't get the left desired results.
I used Foucault differential equations as below : i coded them with Finite difference method (more simple than Runge-Kutta) by replacing the 2nd order derivation by its central finite difference. And the first order one by it's backward finite difference. By then, i build my loop by isolating x(i+1) and y(i+1) in both equations.
My code is very sensitive to parameters such as h (=derivation step), earth angular velocity and latitude (which is normal). I tried to change a way big range of parameters from a big h step to a small one, to a minimal and high latitude, initial conditions...etc but i couldn't ever get the left graphic which i rather need.
What could be made to get the left one ?
I was able to get the two charts, by speeding up the earth's rotation 120× fold, and allowing the simulation to run for 32 swings of the pendulum. Also, I noticed that Euler integration added energy to the system making for bad results, so I reverted to a standard RK4 implementation.
and here is the code I used to solve this ODE:
program FoucaultOde
implicit none
integer, parameter :: sp = kind(1.0), dp = kind(1d0)
! Constants
real, parameter :: g=9.80665, pi =3.1415926536
! Variables
real, allocatable :: y(:,:), yp(:), k0(:),k1(:),k2(:),k3(:)
real :: lat, omega, h, L, earth, period
real :: t0,x0,y0,vx0,vy0
integer :: i, zeta, f1, swings
! Code starts here
swings = 32
zeta = 400*swings
L = 67
lat = 49*pi/180
period = 24*60*60 ! period = 86400
earth = (2*pi*sin(lat)/period)*120 !120 multiplier for roation
omega = sqrt(g/L)
allocate(y(5,zeta))
allocate(yp(5), k0(5),k1(5),k2(5),k3(5))
! make pendulum complete 'swings' cycles in 'zeta' steps
h = swings*2*pi/(omega*zeta)
t0 = 0
x0 = 0.5 ! Initial displacement
y0 = 0
vx0 = 0
vy0 = 0
! Initial conditions in the state vector Y
Y(:,1) = [t0,x0,y0,vx0,vy0]
do i=2, zeta
! Euler method (single step)
! Yp = ode(Y(:,i-1))
! Runge-Kutta method (four steps)
k0 = ode(Y(:,i-1))
k1 = ode(Y(:,i-1) + h/2*k0)
k2 = ode(Y(:,i-1) + h/2*k1)
k3 = ode(Y(:,i-1) + h*k2)
Yp = (k0+2*k1+2*k2+k3)/6
! Take a step
Y(:,i) = Y(:,i-1) + h*Yp
end do
open( newunit=f1, file='results.csv', status = 'replace', pad='no')
! write header
write (f1, '(a15,a,a15,a,a15,a,a15,a,a15)') 't',',', 'x',',','y',',', 'vx',',','vy'
! write rows of data, comma-separated
do i=1, zeta
write (f1, '(g,a,g,a,g,a,g,a,g)') y(1,i),',',y(2,i),',',y(3,i),',',y(4,i),',',y(5,i)
end do
close(f1)
contains
function ode(Y) result(Yp)
real, intent(in) :: Y(5)
real :: Yp(5), t,px,py,vx,vy,ax,ay
! Read state vector Y to component values
t = Y(1)
px = Y(2)
py = Y(3)
vx = Y(4)
vy = Y(5)
! Reference paper:
! http://www.legi.grenoble-inp.fr/people/Achim.Wirth/final_version.pdf
ax = -(omega**2)*px + 2*vy*earth ! (equation 53)
ay = -(omega**2)*py - 2*vx*earth ! (equation 54)
! State vector rate. Note, rate of time is aways 1.0
Yp = [1.0, vx, vy, ax, ay]
end function
end program FoucaultOde
The resulting file results.csv looks like this for me (for checking)
t, x, y, vx, vy
.000000 , 5.000000 , .000000 , .000000 , .000000
.4105792E-01, 4.999383 , .1112020E-06, -.3004657E-01, .8124921E-05
.8211584E-01, 4.997533 , .8895339E-06, -.6008571E-01, .3249567E-04
.1231738 , 4.994450 , .3001796E-05, -.9011002E-01, .7310022E-04
.1642317 , 4.990134 , .7114130E-05, -.1201121 , .1299185E-03
.2052896 , 4.984587 , .1389169E-04, -.1500844 , .2029225E-03
.2463475 , 4.977810 , .2399832E-04, -.1800197 , .2920761E-03
.2874054 , 4.969805 , .3809619E-04, -.2099106 , .3973353E-03
...
from which I plotted the 2nd and 3rd columns in one chart, and the 4th and 5th for the second chart.
There is one thing that may be wrong depending on how you manage different step sizes, and an observation on the physics of the real-world example. With the initialization of the arrays, you imply an initial velocity of about 0.9/0.01=90 [m/s] in x direction away from the center. To get compatible results for different step sizes, you would need to adapt the calculation of x(2). However, in the graphs the plot starts from a point with zero velocity. This you can implement to first order by setting x(2)=x(1)=1. As the used integration method is also first order, this is sufficient.
For the second point, note that one can write the system using complex coordinates z=x+iy as
z'' = -w^2*z - 2*i*E*z', E = Omega*sin(theta)
This is a linear ODE with constant coefficients, the solution of it is
z(t) = exp(-i*E*t) * (A*cos(w1*t)+B*sin(w1*t)), w1 = sqrt(w^2+E^2)
This describes a pendulum motion of frequency w1 whose plane rotates with frequency E clockwise. The grand rotation has period T=2*pi/E, during which w1*T/(2*pi)=w1/E pendulum swings occur.
Now insert your numbers, w=sqrt(g/L)=0.383 and E=2*pi*sin(49°)/86400=5.49e-05, so that essentially w1=w. The number of pendulum cycles per full rotation is w/E=6972, so that you can expect a densely filled circle in the plot. Or a very narrow double wedge if only a few cycles are plotted. As each cycle takes 2*pi/w=16.4 [s], and the integration goes 1000 steps of step size 0.01, in the plot as it is you can expect a swing forth and part of the swing back.
To be more realistic, set the initial velocity to zero, that is, the pendulum is taken to its start position and then let go. Also increase the time to 30 [s] to have more than one pendulum cycle in the plot.
It from this we can see that the solutions converge, and with some imagination, that they converge linearly.
To get a plot like in the cited images, one needs a much smaller fraction of w/E, counting the swings, it has to be around 15. Note that you can not get this ratio anywhere on earth with a realistically scaled pendulum. So set w=pi, E=pi/16 and integrate over 15 time units using the first order method.
This detoriorates really fast, even for the smallest step size with 40 points in a pendulum cycle.
For a better result, increase the local truncation order to the next higher by using the central difference in the first derivative approximation.
z(i+1) - 2*z(i) + z(i-1) = -w^2*z(i)*dt^2 - i*E*(z(i+1)-z(i-1))*dt
z(i+1) = ( 2*z(i) - z(i-1) - w^2*z(i)*dt^2 + i*E*z(i-1)*dt ) / (1+i*E*dt)
The division by the complex number can also be easily carried out in the real components of the trajectory,
! x(i+1)-2*x(i)+x(i-1) = h^2*(-omega**2*x(i)) + h*earth*(y(i+1)-y(i-1))
! y(i+1)-2*y(i)+y(i-1) = h^2*(-omega**2*y(i)) - h*earth*(x(i+1)-x(i-1))
t(i) = t(i-1) + h
cx = (2-(h*omega)**2)*x(i) - x(i-1) - h*earth*y(i-1)
cy = (2-(h*omega)**2)*y(i) - y(i-1) + h*earth*x(i-1)
den = 1+(h*earth)**2
x(i+1) = (cx + h*earth*cy)/den
y(i+1) = (cy - h*earth*cx)/den
Now to respect the increased order, also the initial points need to have an order of accuracy more, using again zero initial speed, this gives in the second order Taylor expansion
z(2) = z(1) - 0.5*w^2*z(1)*dt^2
All the step sizes that gave deviating and structurally deteriorating results in the first order method now give a visually identical, structurally stable results in this second order method.
I have a function that moves a planet around a star. This function takes a parameter t, which is the time in milliseconds since the last update. In other movement functions I've written, I like to use time to dictate movement so the movement will always be the same on all computers and instances instead of based on processing power. However, all methods I have tried for including time in this physics equation have resulted in erratic results. Any ideas?
void Planet::update(int t){
double grav_const = 6.6742e-11;
double earth_mass = 5.975e24;
double starX = 1920/2 * 10000;
double starY = 1080/2 * 10000;
double diffX = xPos - starX;
double diffY = yPos - starY;
double radius = sqrt(pow(diffX,2) + pow(diffY,2));
double grav_accel = (grav_const * (earth_mass / pow(radius,2)));
double angle = atan2(diffX, diffY);
xVel += (sin(angle) * grav_accel);
yVel += (cos(angle) * grav_accel);
xPos -= xVel;
yPos -= yVel;
}
It's been a while since I dealt with physics at this level, but I think you can go back to fundamental reasoning about the units involved.
Acceleration is distance over time squared (m/s^2 or whatever your units are). So to get velocity (distance over time) then you need to multiply by time.
m/s = (m/s^2) * s
And then after that you want to turn your velocity into a specific change in distance. So multiply it by the time again and there you go.
m = (m/s) * s
If things still don't seem right afterwards, then you may need to check over the rest of your equations and constants. Make sure the units match up (seconds vs minutes, metere vs kilometers, etc). Make sure you aren't suffering rounding in places you didn't intend. And so on.
In the worst case, work the math yourself for a few iterations (perhaps with larger time values) and maybe even plot the results on a piece of paper to make sure it looks sensible.
When you describes the results as "erratic" exactly what do you mean?
If you mean:
A. "t changes by a varying amount between each call". Then you need to look at the architecture of the calling application since that will vary with processing power and other work going on in the system (assuming a preemptive multitasking OS).
B. "the floating point values have strange rounding characteristics". Then welcome to using floating point numbers. The representations of double, float and the like are simply imperfect and exhibit rounding areas in certain circumstances and you may have problems if you are taking deltas that are too small relative to the size of the other values you are combining.
C. "t has no effect on my results". I don't see any references to the input parameter in your example.
You should post the entire Planet class, or at least more of it.
EDIT: The best way to calculate position based on times like this is to come up with an absolute function that returns position based on time and NOT accumulate position, but only accumulate time. For example:
timeAbsolute += tDelta;
xPos = fxPos(timeAbsolute);
yPos = fyPos(timeAbsolute);
xVel = fxVel(timeAbsolute);
yVel = fyVel(timeAbsolute);
My orbital mechanics fu is not strong enough to give you those functions in general, but in your case (where you seem to be assuming a circular orbit), you can simply take the arc angle instead. So, assuming 1 orbit every 360 seconds (and using degrees), you would get
angle = (timeAbsolute % 360);
then calc velocity and position from angle.
P.S. Be careful with fmod ...
How to use fmod and avoid precision issues
I have a ground set up of various points, some of which are flat and others are at an angle, I'm trying to check if there is a collision between the angled points (non-axis aligned).
I have a vector array consisting of two floats at each point - This is each of the points of the ground.
Here's an image representation of what the ground looks like.
http://i.imgur.com/cgEMqUv.png?1?4597
At the moment I want to check collisions between points 1 and 2 and then go onto the others.
I shall use points 1 and 2 as an example.
g1x = 150; g2x = 980;
g2x = 500; g2y = 780;
The dxdy of this is dx = 350 and dy = -200
The normal x of this is dy and the normal y is -dx
nx = -200;
ny = -350;
normalized it is the length between points 1 and 2 which is 403.11
nx/normalized = -0.496
ny/normalized = -0.868
//get position of object - Don't know if its supposed to be velocity or not
float vix = object->getPosition().x;
float viy = object->getPosition().y;
//calculate dot product - unsure if vix/viy are supposed to be minused
float dot = ((-vix * nrmx) + (-viy * nrmy)) * nrmx; //= -131.692
Is this information correct to calculate the normal and dot product between the two points.
How can I check if there is a collision with this line and then reflect according to the normal.
Thanks :) any and all changes are welcome.
Say you have a particle at position x travelling at velocity v and a boundary defined by the line between a and b.
We can find how far along the boundary (as a fraction) the particle collides by projecting c-a onto b-a and dividing by the length ||b-a||. That is,
u = ((c-a).((b-a)/||b-a||))/||b-a|| == (c-a).(b-a) / ||b-a||2.
If u > 1 then the particle travels past the boundary on the b side, if u < 0 then the particle travels past the boundary on the a side. The point of collision would be
c = a + u b.
The time to collision could be found by solving
x + t v = a + s (b-a)
for t. The reflection matrix can be found here. But it will need to be rotated by 90 deg (or pi/2) so that you're reflecting orthogonal to the line, not across it.
In terms of multiple boundaries, calculate the time to collision for each of them, sort by that time (discarding negative times) and check for collisions through the list. Once you've found the one that you will collide with then you can move your particle to the point of collision, reflect it's velocity, change the delta t and redo the whole thing again (ignoring the one you just collided with) as you may collide with more than one boundary in a corner case (get it? It's a maths pun).
Linear algebra can be fun, and you can do so much more with it, getting to grips with linear algebra allows you to do some powerful things. Good luck!
I'm currently working on a 3D Rigid Body simulation program. I have currently managed to get the rigid bodies colliding with the floor and bouncing correctly using the impulse. However, my problem is once they have bounced they accelerate constantly despite using a friction vector to try and slow them.
This is the code when you hit the ground
Rvector fDirection(m_Bodies[i].Vel.x,0.0,m_Bodies[i].Vel.z);
Rvector relativeVelocities = m_Bodies[i].Vel - floorVelocity;
fDirection.normalize();
Real impulse = -(1+e) * (Rvector::dotProduct(relativeVelocities,floorNormal))
/ (1/m_Bodies[i].mass + floorMass);
Rvector friction = fDirection*mu*gravity.length()*m_Bodies[i].mass;
Rvector collision_forces = Rvector(0,1,0)*impulse;
collision_forces += friction ;
m_Bodies[i].Vel += (collision_forces/m_Bodies[i].mass);
Thanks
Edit:
Here is the integration code.
void RigidBodySimulation::eulerIntegration(float dTime)
{
Rvector newVel;
Rvector newPos;
Rvector zero(0.0, 0.0, 0.0);
Real one_over_mass;
Rvector accel;
for( unsigned int i = 0 ; i < m_Bodies.size(); i++)
{
one_over_mass = 1/m_Bodies[i].mass;
newVel = m_Bodies[i].Vel + m_Bodies[i].force*one_over_mass*dTime;
newPos = m_Bodies[i].Pos + m_Bodies[i].Vel*dTime;
accel = m_Bodies[i].force / m_Bodies[i].mass;
m_Bodies[i].acceleration = accel;
m_Bodies[i].newPos = newPos;
m_Bodies[i].Vel = newVel;
m_Bodies[i].Pos = newPos;
}
}
I have to say, this is a pretty terrible piece of code you have there, and I've been doing this for more than 10 years. You should get a basic textbook on dynamics (like Hibbeler).
Real impulse = -(1+e) * (Rvector::dotProduct(relativeVelocities,floorNormal))
/ (1/m_Bodies[i].mass + floorMass);
This equation sort of looks like you are trying to calculate the restitution impulse from the impact (although the calculation is wrong). First, you must understand that an impulse is not the same thing as a force. An impulse is the integral of the force over a certain interval of time. During an impact, you can assume that period of time to be really small and that's why you perform an instantaneous velocity change. And that's why you should have specified that the integration code has nothing to do with the collision calculation, because it is by-passed for that instant, or at least, it should be if you do an impulse-based calculation. This is what the actual calculation should look like:
Real momentum_before = Rvector::dotProduct(m_Bodies[i].Vel * m_Bodies[i].mass + floorVelocity * floorMass, floorNormal);
Real rel_vel_after = -e * Rvector::dotProduct(relativeVelocities,floorNormal);
// conservation of momentum in normal direction gives this:
Real body_vel_after = (momentum_before + floorMass * rel_vel_after) / (m_Bodies[i].mass + floorMass);
Real floor_vel_after = body_vel_after - rel_vel_after;
Which actually simplifies to one line as follows:
Real body_vel_after = ( (m_Bodies[i].mass - e * floorMass) * Rvector::dotProduct(m_Bodies[i].Vel, floorNormal)
+ (1.0 + e) * floorMass * Rvector::dotProduct(floorVelocity, floorNormal)
) / (m_Bodies[i].mass + floorMass);
However, if you assume the floor to have infinite mass (or much larger than that of the body), then you would simply have:
Real body_rel_vel_after = -e * Rvector::dotProduct(relativeVelocities, floorNormal);
Real body_vel_after = Rvector::dotProduct(floorVelocity, floorNormal) + body_rel_vel_after;
It's that simple. But, under that assumption, you do not have conservation of momentum. But in any case, the restitution impulse from the impact can be calculated as:
Real impulse = m_Bodies[i].mass * (body_vel_after - Rvector::dotProduct(m_Bodies[i].Vel, floorNormal));
Now, because the restitution impulse is the integral of the normal force over the small time period of impact, the impulse from the friction during the impact can be calculated from that restitution impact. The friction force is equal to "mu" times the normal force, i.e., |Ff| = mu * |Fn|, this is also valid for the impulse, i.e., |If| = mu * |In|. So, you can compute it directly:
Real friction_impulse = mu * fabs(impulse);
But that's just the magnitude of the friction impulse. It's direction is opposite from the relative tangential velocity, which is:
Rvector tangent_rel_vel = relativeVelocities - Rvector::dotProduct(relativeVelocities, floorNormal) * floorNormal;
And it's direction is:
Rvector dir_rel_vel = tangent_rel_vel;
dir_rel_vel.normalize();
(Notice that I need to keep the tangential velocity intact, because it will be needed later)
At this point, you could compute the tangential velocity after impact as follows (again, under the assumption of an infinite-mass floor, otherwise, it is more complicated than that):
Rvector tangent_rel_vel_after = tangent_rel_vel - dir_rel_vel * friction_impulse / m_Bodies[i].mass;
However, what if the friction impulse causes the tangential relative velocity to get to zero? That's a problem, because, with the above formula, part of the friction impulse could wind up reversing the direction of the tangential relative velocity, which would mean that during the latter part of the impact, the friction force is actually acting in the direction of the velocity (not good). The most friction can do is stop the relative motion. So, you need to check for that condition:
Real tang_rel_vel_change = friction_impulse / mBodies[i].mass;
Rvector tangent_rel_vel_after = tangent_rel_vel - dir_rel_vel * tang_rel_vel_change;
if ( tang_rel_vel_change > tangent_rel_vel.length() )
tangent_rel_vel_after = Rvector(0.0, 0.0, 0.0); // stop relative motion.
At this point, all you need to do is combine the two final velocities:
m_Bodies[i].Vel = floorVelocity + tangent_rel_vel_after + body_rel_vel_after * floorNormal;
And that's it, at least, for this very simple problem (infinite mass of the floor). In reality, this impulse-based approach become increasingly difficult to deal with as you complicate things: two finite-mass objects, multiple objects, and actual rigid-body dynamics (because you are just doing particle dynamics here). The impulse-based approach is rarely seen anywhere beyond simple schoolyard examples of balls bouncing on the floor. Btw, you shouldn't really call this a "rigid-body" simulator since all you are really doing a particle dynamics (and 3D rigid-body dynamics is way more complicated than this). Also, your integration law is terrible, but that's a whole different story.
Friction is backwards, isn't it?
Rvector fDirection(m_Bodies[i].Vel.x,0.0,m_Bodies[i].Vel.z);
This is in the direction of the velocity. You then multiply it by some constants then add it to the velocity (along with the impulse)
collision_forces += friction ;
m_Bodies[i].Vel += (collision_forces/m_Bodies[i].mass);
So that will act to increase the velocity along the floor.
There are some other odd things:
your impulse term has: (1/m_Bodies[i].mass + floorMass) This is adding 1/mass to mass. Shouldn't it be (1/(m_Bodies[i].mass + floorMass))
And then in the integrator, you calculate acceleration from force, but then never integrate it, you also apply force to velocity directly. So what's the acceleration member for?
Is there a way to do a quick and dirty 3D distance check where the results are rough, but it is very very fast? I need to do depth sorting. I use STL sort like this:
bool sortfunc(CBox* a, CBox* b)
{
return a->Get3dDistance(Player.center,a->center) <
b->Get3dDistance(Player.center,b->center);
}
float CBox::Get3dDistance( Vec3 c1, Vec3 c2 )
{
//(Dx*Dx+Dy*Dy+Dz*Dz)^.5
float dx = c2.x - c1.x;
float dy = c2.y - c1.y;
float dz = c2.z - c1.z;
return sqrt((float)(dx * dx + dy * dy + dz * dz));
}
Is there possibly a way to do it without a square root or possibly without multiplication?
You can leave out the square root because for all positive (or really, non-negative) numbers x and y, if sqrt(x) < sqrt(y) then x < y. Since you're summing squares of real numbers, the square of every real number is non-negative, and the sum of any positive numbers is positive, the square root condition holds.
You cannot eliminate the multiplication, however, without changing the algorithm. Here's a counterexample: if x is (3, 1, 1) and y is (4, 0, 0), |x| < |y| because sqrt(1*1+1*1+3*3) < sqrt(4*4+0*0+0*0) and 1*1+1*1+3*3 < 4*4+0*0+0*0, but 1+1+3 > 4+0+0.
Since modern CPUs can compute a dot product faster than they can actually load the operands from memory, it's unlikely that you would have anything to gain by eliminating the multiply anyway (I think the newest CPUs have a special instruction that can compute a dot product every 3 cycles!).
I would not consider changing the algorithm without doing some profiling first. Your choice of algorithm will heavily depend on the size of your dataset (does it fit in cache?), how often you have to run it, and what you do with the results (collision detection? proximity? occlusion?).
What I usually do is first filter by Manhattan distance
float CBox::Within3DManhattanDistance( Vec3 c1, Vec3 c2, float distance )
{
float dx = abs(c2.x - c1.x);
float dy = abs(c2.y - c1.y);
float dz = abs(c2.z - c1.z);
if (dx > distance) return 0; // too far in x direction
if (dy > distance) return 0; // too far in y direction
if (dz > distance) return 0; // too far in z direction
return 1; // we're within the cube
}
Actually you can optimize this further if you know more about your environment. For example, in an environment where there is a ground like a flight simulator or a first person shooter game, the horizontal axis is very much larger than the vertical axis. In such an environment, if two objects are far apart they are very likely separated more by the x and y axis rather than the z axis (in a first person shooter most objects share the same z axis). So if you first compare x and y you can return early from the function and avoid doing extra calculations:
float CBox::Within3DManhattanDistance( Vec3 c1, Vec3 c2, float distance )
{
float dx = abs(c2.x - c1.x);
if (dx > distance) return 0; // too far in x direction
float dy = abs(c2.y - c1.y);
if (dy > distance) return 0; // too far in y direction
// since x and y distance are likely to be larger than
// z distance most of the time we don't need to execute
// the code below:
float dz = abs(c2.z - c1.z);
if (dz > distance) return 0; // too far in z direction
return 1; // we're within the cube
}
Sorry, I didn't realize the function is used for sorting. You can still use Manhattan distance to get a very rough first sort:
float CBox::ManhattanDistance( Vec3 c1, Vec3 c2 )
{
float dx = abs(c2.x - c1.x);
float dy = abs(c2.y - c1.y);
float dz = abs(c2.z - c1.z);
return dx+dy+dz;
}
After the rough first sort you can then take the topmost results, say the top 10 closest players, and re-sort using proper distance calculations.
Here's an equation that might help you get rid of both sqrt and multiply:
max(|dx|, |dy|, |dz|) <= distance(dx,dy,dz) <= |dx| + |dy| + |dz|
This gets you a range estimate for the distance which pins it down to within a factor of 3 (the upper and lower bounds can differ by at most 3x). You can then sort on, say, the lower number. You then need to process the array until you reach an object which is 3x farther away than the first obscuring object. You are then guaranteed to not find any object that is closer later in the array.
By the way, sorting is overkill here. A more efficient way would be to make a series of buckets with different distance estimates, say [1-3], [3-9], [9-27], .... Then put each element in a bucket. Process the buckets from smallest to largest until you reach an obscuring object. Process 1 additional bucket just to be sure.
By the way, floating point multiply is pretty fast nowadays. I'm not sure you gain much by converting it to absolute value.
I'm disappointed that the great old mathematical tricks seem to be getting lost. Here is the answer you're asking for. Source is Paul Hsieh's excellent web site: http://www.azillionmonkeys.com/qed/sqroot.html . Note that you don't care about distance; you will do fine for your sort with square of distance, which will be much faster.
In 2D, we can get a crude approximation of the distance metric without a square root with the formula:
distanceapprox (x, y) =
which will deviate from the true answer by at most about 8%. A similar derivation for 3 dimensions leads to:
distanceapprox (x, y, z) =
with a maximum error of about 16%.
However, something that should be pointed out, is that often the distance is only required for comparison purposes. For example, in the classical mandelbrot set (z←z2+c) calculation, the magnitude of a complex number is typically compared to a boundary radius length of 2. In these cases, one can simply drop the square root, by essentially squaring both sides of the comparison (since distances are always non-negative). That is to say:
√(Δx2+Δy2) < d is equivalent to Δx2+Δy2 < d2, if d ≥ 0
I should also mention that Chapter 13.2 of Richard G. Lyons's "Understanding Digital Signal Processing" has an incredible collection of 2D distance algorithms (a.k.a complex number magnitude approximations). As one example:
Max = x > y ? x : y;
Min = x < y ? x : y;
if ( Min < 0.04142135Max )
|V| = 0.99 * Max + 0.197 * Min;
else
|V| = 0.84 * Max + 0.561 * Min;
which has a maximum error of 1.0% from the actual distance. The penalty of course is that you're doing a couple branches; but even the "most accepted" answer to this question has at least three branches in it.
If you're serious about doing a super fast distance estimate to a specific precision, you could do so by writing your own simplified fsqrt() estimate using the same basic method as the compiler vendors do, but at a lower precision, by doing a fixed number of iterations. For example, you can eliminate the special case handling for extremely small or large numbers, and/or also reduce the number of Newton-Rapheson iterations. This was the key strategy underlying the so-called "Quake 3" fast inverse square root implementation -- it's the classic Newton algorithm with exactly one iteration.
Do not assume that your fsqrt() implementation is slow without benchmarking it and/or reading the sources. Most modern fsqrt() library implementations are branchless and really damned fast. Here for example is an old IBM floating point fsqrt implementation. Premature optimization is, and always will be, the root of all evil.
Note that for 2 (non-negative) distances A and B, if sqrt(A) < sqrt(B), then A < B. Create a specialized version of Get3DDistance() (GetSqrOf3DDistance()) that does not call sqrt() that would be used only for the sortfunc().
If you worry about performance, you should also take care of the way you send your arguments:
float Get3dDistance( Vec3 c1, Vec3 c2 );
implies two copies of Vec3 structure. Use references instead:
float Get3dDistance( Vec3 const & c1, Vec3 const & c2 );
You could compare squares of distances instead of the actual distances, since d2 = (x1-x2)2 + (y1-y2)2+ (z1-z2)2. It doesn't get rid of the multiplication, but it does eliminate the square root operation.
How often are the input vectors updated and how often are they sorted? Depending on your design, it might be quite efficient to extend the "Vec3" class with a pre-calculated distance and sort on that instead. Especially relevant if your implementation allows you to use vectorized operations.
Other than that, see the flipcode.com article on approximating distance functions for a discussion on yet another approach.
Depending slightly on the number of points that you are being used to compare with, what is below is pretty much guaranteed to be the get the list of points in approximate order assuming all points change at all iteration.
1) Rewrite the array into a single list of Manhattan distances with
out[ i ] = abs( posn[ i ].x - player.x ) + abs( posn[ i ].y - player.y ) + abs( posn[ i ].z - player.z );
2) Now you can use radix sort on floating point numbers to order them.
Note that in practice this is going to be a lot faster than sorting the list of 3d positions because it significantly reduces the memory bandwidth requirements in the sort operation which all of the time is going to be spend and in which unpredictable accesses and writes are going to occur. This will run on O(N) time.
If many of the points are stationary at each direction there are far faster algorithms like using KD-Trees, although implementation is quite a bit more complex and it is much harder to get good memory access patterns.
If this is simply a value for sorting, then you can swap the sqrt() for a abs(). If you need to compare distances against set values, get the square of that value.
E.g. instead of checking sqrt(...) against a, you can compare abs(...) against a*a.
You may want to consider caching the distance between the player and the object as you calculate it, and then use that in your sortfunc. This would depend upon how many times your sort function looks at each object, so you might have to profile to be sure.
What I'm getting at is that your sort function might do something like this:
compare(a,b);
compare(a,c);
compare(a,d);
and you would calculate the distance between the player and 'a' every time.
As others have mentioned, you can leave out the sqrt in this case.
If you could center your coordinates around the player, use spherical coordinates? Then you could sort by the radius.
That's a big if, though.
If your operation happens a lot, it might be worth to put it into some 3D data structure. You probably need the distance sorting to decide which object is visible, or some similar task. In order of complexity you can use:
Uniform (cubic) subdivision
Divide the used space into cells, and assign the objects to the cells. Fast access to element, neighbours are trivial, but empty cells take up a lot of space.
Quadtree
Given a threshold, divide used space recursively into four quads until less then threshold number of object is inside. Logarithmic access element if objects don't stack upon each other, neighbours are not hard to find, space efficient solution.
Octree
Same as Quadtree, but divides into 8, optimal even if objects are above each other.
Kd tree
Given some heuristic cost function, and a threshold, split space into two halves with a plane where the cost function is minimal. (Eg.: same amount of objects at each side.) Repeat recursively until threshold reached. Always logarithmic, neighbours are harder to get, space efficient (and works in all dimensions).
Using any of the above data structures, you can start from a position, and go from neighbour to neighbour to list the objects in increasing distance. You can stop at desired cut distance. You can also skip cells that cannot be seen from the camera.
For the distance check, you can do one of the above mentioned routines, but ultimately they wont scale well with increasing number of objects. These can be used to display data that takes hundreds of gigabytes of hard disc space.