I have this runge kutta code. However, one mentioned my approach is wrong. And I couldn't really understand why from him, so anyone here, who could give a hint on why this way is wrong?
Vector3d r = P.GetAcceleration();
Vector3d s = P.GetAcceleration() + 0.5*m_dDeltaT*r;
Vector3d t = P.GetAcceleration() + 0.5*m_dDeltaT*s;
Vector3d u = P.GetAcceleration() + m_dDeltaT*t;
P.Velocity += m_dDeltaT * (r + 2.0 * (s + t) + u) / 6.0);
====EDIT====
Vector3d are storing the coordinates, x, y, z.
The GetAcceleration returns the acceleration for each x, y, and z.
You have some acceleration function
a(p,q) where p=(x,y,z) and q=(vx,vy,vz)
Your order 1 system that can be solved via RK4 is
dotp = q
dotq = a(p,q)
The stages of the RK method involve an offset of the state vector(s)
k1p = q
k1q = a(p,q)
p1 = p + 0.5*dt*k1p
q1 = q + 0.5*dt*k1q
k2p = q1
k2q = a(p1,q1)
p2 = p + 0.5*dt*k2p
q2 = p + 0.5*dt*k2q
k3p = q2
k3q = a(p2,q2)
etc. You can either adjust the state vectors of the point P for each step, saving the original coordinates, or use a temporary copy of P to compute k2, k3, k4.
You haven't defined your methods, but the thing that's jumping out at me is you're mixing your results with your inputs. Since Runge-Kutta is a method for calculating y_(n+1) = y_n + hsum(b_ik_i), I would expect your solution to keep your _n terms on the right, and your (n+1) terms on the left. This is NOT what you're doing. Instead, s(n+1) is dependent on r_(n+1) instead of on r_n, t_(n+1) on s_(n+1), and so on. This smells of an error where you attempted to limit the number of variables being used.
With that in mind, can you indicate the actual intermediate values of the calculations your program generates and compare them with the intended intermediate values?
Related
I have to use this formula in order to draw 3rd degree b-spline curve
Can someone give me advice what am I doing wrong in my code? Doesn't seem to work properly for me and I am getting this weird results when trying to draw the curve
segment is a vector of QPoint, it has x and y
void MyWindow::calculateCurve() {
QPoint result;
int m = segment.size();
int from = m-3;
int to = m-2;
for(double t = 0.0; t<=to; t+=0.001){
result = (pow(-t, 3)+3*pow(t,2)+1)/6*(segment[segment.size()-3])+
(3*pow(t,3)-6*pow(t,2)+4)/6*(segment[segment.size()-2])+
(pow(-3*t,3)+3*pow(t,2)+3*t+1)/6*(segment[segment.size()-1]) +
(pow(t,3)/6)*(segment[segment.size()])
;
draw(result.x(), result.y());
}
}
Most often we define a common range for the parameter t (i.e. for
the whole curve, not for each segment separately). We can
e.g. assume that t ∈ [0, m - 2]. Then, for the segment Q3
parameter t varies from t3 = 0 to t4 = 1, for segment
Q4 from t4 = 1 to t5 = 2, and for the last segment Qm from
tm = m - 3 to tm+1 = m - 2.
You have written pow(-3*t,3), which means (-3t)³, but you should have written -3*pow(t,3), that is, -3(t³):
result = (pow(-t, 3)+3*pow(t,2)+1)/6*(segment[segment.size()-3])+
(3*pow(t,3)-6*pow(t,2)+4)/6*(segment[segment.size()-2])+
(-3*pow(t,3)+3*pow(t,2)+3*t+1)/6*(segment[segment.size()-1]) +
(pow(t,3)/6)*(segment[segment.size()])
;
Example: let
M = Matrix([[1,2],[3,4]]) # and
p = Poly(x**3 + x + 1) # then
p.subs(x,M).expand()
gives the error :
TypeError: cannot add <class'sympy.matrices.immutable.ImmutableDenseMatrix'> and <class 'sympy.core.numbers.One'>
which is very plausible since the two first terms become matrices but the last term (the constant term) is not a matrix but a scalar. To remediate to this situation I changed the polynomial to
p = Poly(x**3 + x + x**0) # then
the same error persists. Am I obliged to type the expression by hand, replacing x by M? In this example the polynomial has only three terms but in reality I encounter (multivariate polynomials with) dozens of terms.
So I think the question is mainly revolving around the concept of Matrix polynomial:
(where P is a polynomial, and A is a matrix)
I think this is saying that the free term is a number, and it cannot be added with the rest which is a matrix, effectively the addition operation is undefined between those two types.
TypeError: cannot add <class'sympy.matrices.immutable.ImmutableDenseMatrix'> and <class 'sympy.core.numbers.One'>
However, this can be circumvented by defining a function that evaluates the matrix polynomial for a specific matrix. The difference here is that we're using matrix exponentiation, so we correctly compute the free term of the matrix polynomial a_0 * I where I=A^0 is the identity matrix of the required shape:
from sympy import *
x = symbols('x')
M = Matrix([[1,2],[3,4]])
p = Poly(x**3 + x + 1)
def eval_poly_matrix(P,A):
res = zeros(*A.shape)
for t in enumerate(P.all_coeffs()[::-1]):
i, a_i = t
res += a_i * (A**i)
return res
eval_poly_matrix(p,M)
Output:
In this example the polynomial has only three terms but in reality I encounter (multivariate polynomials with) dozens of terms.
The function eval_poly_matrix above can be extended to work for multivariate polynomials by using the .monoms() method to extract monomials with nonzero coefficients, like so:
from sympy import *
x,y = symbols('x y')
M = Matrix([[1,2],[3,4]])
p = poly( x**3 * y + x * y**2 + y )
def eval_poly_matrix(P,*M):
res = zeros(*M[0].shape)
for m in P.monoms():
term = eye(*M[0].shape)
for j in enumerate(m):
i,e = j
term *= M[i]**e
res += term
return res
eval_poly_matrix(p,M,eye(M.rows))
Note: Some sanity checks, edge cases handling and optimizations are possible:
The number of variables present in the polynomial relates to the number of matrices passed as parameters (the former should never be greater than the latter, and if it's lower than some logic needs to be present to handle that, I've only handled the case when the two are equal)
All matrices need to be square as per the definition of the matrix polynomial
A discussion about a multivariate version of the Horner's rule features in the comments of this question. This might be useful to minimize the number of matrix multiplications.
Handle the fact that in a Matrix polynomial x*y is different from y*x because matrix multiplication is non-commutative . Apparently poly functions in sympy do not support non-commutative variables, but you can define symbols with commutative=False and there seems to be a way forward there
About the 4th point above, there is support for Matrix expressions in SymPy, and that can help here:
from sympy import *
from sympy.matrices import MatrixSymbol
A = Matrix([[1,2],[3,4]])
B = Matrix([[2,3],[3,4]])
X = MatrixSymbol('X',2,2)
Y = MatrixSymbol('Y',2,2)
I = eye(X.rows)
p = X**2 * Y + Y * X ** 2 + X ** 3 - I
display(p)
p = p.subs({X: A, Y: B}).doit()
display(p)
Output:
For more developments on this feature follow #18555
I'm trying to implement the Runge Kutta method in Fortran and am facing a convergence problem. I don't know how much of the code I should show, so I'll describe the problem in detail, and please guide me as to what I should add/remove to/from the post to make it answerable.
I have a 6-dimensional vector of position and velocity of a ball, and a corresponding system of diff. eqs. that describe the equations of motions, from which I want to calculate the trajectory of the ball, and compare results for different orders of the RK method.
Let's focus on 3rd order RK. The model I use is implemented as follows:
k1 = h * f(vec_old,omega,phi)
k2 = h * f(vec_old + 0.5d0 * k1,omega,phi)
k3 = h * f(vec_old + 2d0 * k2 - k1,omega,phi)
vec = vec_old + (k1 + 4d0 * k2 + k3) / 6d0
Where f is the function that constitutes the equations of motion (or equivalently the RHS of my system of diff. eqs). Note that f is time independent, therefore has only 1 argument. h takes the role of a small time step dt.
If we wish to calculate the trajectory of the ball for a finite time total_time, and allow for a total error of epsilon, then we need to ensure each step takes a proportional fraction of the error. For the first step, I then did the following:
vec1 = solve(3,vec_old,h,omega,phi)
vec2 = solve(3,vec_old,h/2d0,omega,phi)
do while (maxval((/(abs(vec1(i) - vec2(i)),i=1,6)/)) > eps * h / (tot_time - current_time))
h = h / 2d0
vec1 = solve(3,vec_old,h,omega,phi)
vec2 = solve(3,vec_old,h/2d0,omega,phi)
end do
vec = (8d0/7d0) * vec2 - (1d0/7d0) * vec1
Where solve(3,vec_old,h,omega,phi) is the function that calculates the single RK step described above. 3 denotes the RK order we are using, vec_old is the current state of the position-velocity vector, h, h/2d0 both represent the time step being used, and omega,phi are just some extra parameters for f. Finally, for the first step we set current_time = 0d0.
The point is that if we use a 3rd order RK, we should have an error in $O(h^3)$, and thus fall off faster than linearly in h. Therefore, we should expect the while loop to eventually come to a halt for small enough h.
My problem is that the loop doesn't converge, and not even close - the ratio
maxval(...) / eps * (...)
remains pretty much constant, all the way until eps * h / (tot_time - current_time)) becomes zero due to finite precision.
For completeness, this is my definition for f:
function f(vec_old,omega,phi) result(vec)
real(8),intent(in) :: vec_old(6),omega,phi
real(8) :: vec(6)
real(8) :: v,Fv
v = sqrt(vec_old(4)**2+vec_old(5)**2+vec_old(6)**2)
Fv = 0.0039d0 + 0.0058d0 / (1d0 + exp((v-35d0)/5d0))
vec(1) = vec_old(4)
vec(2) = vec_old(5)
vec(3) = vec_old(6)
vec(4) = -Fv * v * vec_old(4) + 4.1d-4 * omega * (vec_old(6)*sin(phi) - vec_old(5)*cos(phi))
vec(5) = -Fv * v * vec_old(5) + 4.1d-4 * omega * vec_old(4)*cos(phi)
vec(6) = -Fv * v * vec_old(6) - 4.1d-4 * omega * vec_old(4)*sin(phi) - 9.8d0
end function f
Does anyone have any idea as to why the while loop doesn't converge?
If anything else is needed (output, other pieces of code etc.) please tell me and I'll add it. Also, if trimming is required, I'll cut whatever would be considered unnecessary. Thanks!
To compute the step error using the half step method, you need to compute the approximation at t+h in both cases, which means two steps with step size h/2. As it is now you compare the approximation at t+h to the approximation at t+h/2 which gives you an error of size f(vec(t+h/2))*h/2.
Thus change to a 3-step procedure
vec1 = solve(3,vec_old,h,omega,phi)
vec2 = solve(3,vec_old,h/2d0,omega,phi)
vec2 = solve(3,vec2 ,h/2d0,omega,phi)
in both locations, the difference of vec2-vec1 should then be of order h^4.
I have this function to reach a certain 1 dimensional value accelerated and damped with overshoot. That is: given an inital value, a velocity and a acceleration (force/mass), the target value is attained by accelerating to it and gets increasingly damped while getting closer to the target value.
This all works fine, howver If i want to know what the TotalAngle is after time 't' I have to run this function say N steps with a 'small' dt to find the 'limit'.
I was wondering If i can (and how) to intergrate over dt so that the TotalAngle can be determined given a time 't' initially.
Regards, Tanks for any help.
dt = delta time step per frame
input = 1
TotalAngle = 0 at t=0
Velocity = 0 at t=0
void FAccelDampedWithOvershoot::Update(float dt, float input, float& Velocity, float& TotalAngle)
{
const float Force = 500000.f;
const float DampForce = 5000.f;
const float MaxAngle = 45.f;
const float InvMass = 1.f / 162400.f;
float target = MaxAngle * input;
float ratio = (target - TotalAngle) / MaxAngle;
float fMove = Force * ratio;
float fDamp = -Velocity * DampForce;
Velocity += (fMove + fDamp) * invMass * dt;
TotalAngle += Velocity * dt;
}
Updated with fixed bugs in math
Originally I've lost mass and MaxAngle a few times. This is why you should first solve it on a paper and then enter to the SO rather than trying to solve it in the text editor.
Anyway, I've fixed the math and now it seems to work reasonably well. I put fixed solution just over previous one.
Well, this looks like a Newtonian mechanics which means differential equations. Let's try to solve them.
SO is not very friendly to math formulas and I'm a bit bored to type characters so here is what I use:
F = Force
Fd = DampForce
MA = MaxAngle
A= TotalAngle
v = Velocity
m = 1 / InvMass
' for derivative i.e. something' is 1-st derivative of something by t and something'' is 2-nd derivative
if I divide you last two lines of code by dt and merge in all the other lines I can get (I also assume that input = 1 as other case is obviously symmetrical)
v' = ([F * (1 - A / MA)] - v * Fd) / m
and applying A' = v we get
m * A'' = F(1 - A/MA) - Fd * A'
or moving to one side we get a simple 2-nd order differential equation
m * A'' + Fd * A' + F/MA * A = F
IIRC, the way to solve it is to first solve characteristic equation which here is
m * x^2 + Fd * x + F/MA = 0
x[1,2] = (-Fd +/- sqrt(Fd^2 - 4*F*m/MA))/ (2*m)
I expect that part under sqrt i.e. (Fd^2 - 4*F*m/MA) is negative thus solution should be of the following form. Let
Dm = Fd/(2*m)
K = sqrt(F/MA/m - Dm^2)
(note the negated value under sqrt so it works now) then
A(t) = e^(-Dm*t) * [P * sin(K*t) + Q * cos(K*t)] + C
where P, Q and C are some constants.
The solution is easier to find as a sum of two solutions: some specific solution for
m * A'' + Fd * A' + F/MA * A = F
and a general solution for homogeneou
m * A'' + Fd * A' + F/MA * A = 0
that makes original conditions fit. Obviously specific solution A(t) = MA works and thus C = MA. So now we need to fit P and Q of general solution to match starting conditions. To find them we need
A(0) = - MA
A'(0) = V(0) = 0
Given that e^0 = 1, sin(0) = 0 and cos(0) = 1 you get something like
Q = -MA
P = 0
or
P = 0
Q = - MA
C = MA
thus
A(t) = MA * [1 - e^(-Dm*t) * cos(K*t)]
where
Dm = Fd/(2*m)
K = sqrt(F/MA/m - Dm^2)
which kind of makes sense given your task.
Note also that this equation assumes that everything happens in radians rather than degrees (i.e. derivative of [sin(t)]' is just cos(t)) so you should transform all your constants accordingly or transform the solution.
const float Force = 500000.f * M_PI / 180;
const float DampForce = 5000.f * M_PI / 180;
const float MaxAngle = M_PI_4;
which on my machine produces
Dm = 0.000268677541
K = 0.261568546
This seems to be similar to original funcion is I step with dt = 0.01f and the main obstacle seems to be precision loss because of float
Hope this helps!
This is not a full answer and I am sure someone else can work it out, but there is no room in the comments and it may help you find a better solution.
The image below shows the velocity (blue) as your function integrates at time steps 1. The red shows the function below that calculates the value for time t
The function F(t)
F(t) = sin((t / f) * pi * 2) * (1 / (((t / f) + a) ^ c)) * b
With f = 23.7, a = 1.4, c = 2, and b= 50 that give the red plot in the image above
All the values are just approximation.
f determines the frequency and is close to a match,
a,b,c control the falloff in amplitude and are a by eye guestimate.
If it does not matter that you have a perfect match then this will work for you. totalAngle uses the same function but t has 0.25 added to it. Unfortunately I did not get any values for a,b,c for totalAngle and I did notice that it was offset so you will have to add the offset value d (I normalised everything so have no idea what the range of totalAngle was)
Function F(t) for totalAngle
F(t) = sin(((t+0.25) / f) * pi * 2) * (1 / ((((t+0.25) / f) + a) ^ c)) * b + d
Sorry only have f = 23.7, c= 2, a~1.4 nothing for b=? d=?
Does anyone know any c++/opengl sourcecode demos for 2D rigid body physics using runge kutta?
I want to build a physics engine but I need some reference code to understand better how others have implemented this.
There are a lot of things you have to take care to do this nicely. I will focus on the integrator implementation and what I have found works good for me.
For all the degrees of freedom in your system implement a function to return the accelerations a as a function of time t, positions x and velocities v. This should operate on arrays or vectors of quantities and not just scalars.
a = accel(t,x,v);
After each RK step evaluate the acceleration to be ready for the next step. In the loop then do this:
{
// assume t,x[],v[], a[] are known
// step time t -> t+h and calc new values
float h2=h/2;
vec q1 = v + h2*a;
vec k1 = accel(t+h2, x+h2*v, q1);
vec q2 = v + h2*k1;
vec k2 = accel(t+h2, x+h2*q1, q2);
vec q3 = v + h*k2;
vec k3 = accel(t_h, x+h*q2, q3);
float h6 = h/6;
t = t + h;
x = x + h*(v+h6*(a+k1+k2));
v = v + h6*(a+2*k1+2*k2+k3);
a = accel(t,x,v);
}
Why? Well the standard RK method requires you to make a 2xN state vector, but the derivatives of the fist N elements are equal to the last N elements. If you split the problem up to two N state vectors and simplify a little you will arrive at the above scheme for 2nd order RK.
I have done this and the results are identical to commercial software for a plan system with N=6 degrees of freedom.