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()])
;
Related
A line in the 2D plane can be represented with the implicit equation
f(x,y) = a*x + b*y + c = 0
= dot((a,b,c),(x,y,1))
If a^2 + b^2 = 1, then f is considered normalized and f(x,y) gives you the Euclidean (signed) distance to the line.
Say you are given a 3xK matrix (in Eigen) where each column represents a line:
Eigen::Matrix<float,3,Eigen::Dynamic> lines;
and you wish to normalize all K lines. Currently I do this a follows:
for (size_t i = 0; i < K; i++) { // for each column
const float s = lines.block(0,i,2,1).norm(); // s = sqrt(a^2 + b^2)
lines.col(i) /= s; // (a, b, c) /= s
}
I know there must be a more clever and efficient method for this that does not require looping. Any ideas?
EDIT: The following turns out being slower for optimized code... hmmm..
Eigen::VectorXf scales = lines.block(0,0,2,K).colwise().norm().cwiseInverse()
lines *= scales.asDiagonal()
I assume that this as something to do with creating KxK matrix scales.asDiagonal().
P.S. I could use Eigen::Hyperplane somehow, but the docs seem little opaque.
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?
I am working on a object tracking project and I want to improve the results I am getting using a Kalman filter.
I have found a lot of examples on the internet which are working but I really want to understand what is behind it.
Using opencv, here is a part of the code :
KalmanFilter KF(6, 2, 0);
Mat_ state(6, 1);
Mat processNoise(6, 1, CV_32F);
...
KF.statePre.at(0) = mouse_info.x;
KF.statePre.at(1) = mouse_info.y;
KF.statePre.at(2) = 0;
KF.statePre.at(3) = 0;
KF.statePre.at(4) = 0;
KF.statePre.at(5) = 0;
KF.transitionMatrix = *(Mat_(6, 6) << 1,0,1,0,0.5,0, 0,1,0,1,0,0.5, 0,0,1,0,1,0, 0,0,0,1,0,1, 0,0,0,0,1,0, 0,0,0,0,0,1);
KF.measurementMatrix = *(Mat_(2, 6) << 1,0,1,0,0.5,0, 0,1,0,1,0,0.5);
This one gives smoother results than a KalmanFilter(4,2,0) but I don't really understand why.
Can someone explain me what is behind this (6,6) transition matrix ?
EDIT : The solution is probably here but obviously I am not good enough to find it by myself ...
Thank you for your help.
You have a state vector X made up of 6 components, the first two of which are the x and y position of an object; let's assume that the other 4 are their velocities and accelerations:
X = [x, y, v_x, v_y, a_x, a_y] t
In the Kalman filter, your next state, Xt+1, is equal to the previous state Xt multiplied by the transition matrix A, so with the transition matrix you posted, you would have:
x t+1 = x t + v_x t + 0.5 a_x t
y t+1 = y t + v_y t + 0.5 a_y t
v_x t+1 = v_x t + a_x t
v_y t+1 = v_t t + a_t t
a_x t+1 = a_x t
a_y t+1 = a_y t
Which are the discrete approximation of the equations of an object moving with constant acceleration if the time interval between the two states is equal to 1 (and that's why it makes sense to suppose that the other four variables are velocities and accelerations).
This is a Kalman filter that allows for faster variations in the velocity estimation, so it introduces a lower delay than a (4, 2, 0) filter, which would use a constant velocity model.
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.
I'm trying to implement at line-plane intersection algorithm. According to Wikipedia I need three non-colinear points on the plane to do that.
I therefore tried implementing this algorithm in C++, however. Something is definitely wrong, cause it makes no sense that I can choose whatever x and y coordinates and they'll fit in the plane. What if the plane is vertical and along the x-axis? No point with y=1 would then be in the plane.
I realize that this problem has been posted a lot on StackOverflow, and I see lots of solutions where the plane is defined by 3 points. But I only have a normal and a position. And I can't test my line-plane intersection algorithm before I sort out my non-colinear point finder.
The problem right now, is that I'm dividing by normal.z, and that obviously won't work when normal.z is 0.
I'm testing with this plane: Plane* p = new Plane(Color(), Vec3d(0.0,0.0,0.0), Vec3d(0.0,1.0,0.0)); // second parameter : position, third parameter : normal
The current code gives this incorrect answer:
{0 , 0 , 0} // alright, this is the original
{12.8377 , 17.2728 , -inf} // obviously this is not a non-colinear point on the given plane
Here's my code:
std::vector<Vec3d>* Plane::getThreeNonColinearPoints() {
std::vector<Vec3d>* v = new std::vector<Vec3d>();
v->push_back(Vec3d(position.x, position.y, position.z)); // original position can serve as one of the three non-colinear points.
srandom(time(NULL));
double rx, ry, rz, start;
rx = Plane::fRand(10.0, 20.0);
ry = Plane::fRand(10.0, 20.0);
// Formula from here: http://en.wikipedia.org/wiki/Plane_(geometry)#Definition_with_a_point_and_a_normal_vector
// nx(x-x0) + ny(y-y0) + nz(z-z0) = 0
// |-----------------| <- this is "start"
//I'll try to insert position as x0,y0,z0 and normal as nx,ny,nz, and solve the equation
start = normal.x * (rx - position.x) + normal.y * (ry - position.y);
// nz(z-z0) = -start
start = -start;
// (z-z0) = start/nz
start /= normal.z; // division by zero
// z = start+z0
start += position.z;
rz = start;
v->push_back(Vec3d(rx, ry, rz));
// TODO one more point
return v;
}
I realize that I might be trying to solve this totally wrong. If so, please link a concrete implementation of this. I'm sure it must exist, when I see so many line-plane intersection implementations.
Thanks in advance.
A plane can be defined with several ways. Typically a point on the plane and a normal vector is used. To get the normal vector from three points (P1, P2, P3 ) take the cross product of the side of the triangle
P1 = {x1, y1, z1};
P2 = {x2, y2, z2};
P3 = {x3, y3, z3};
N = UNIT( CROSS( P2-P1, P3-P1 ) );
Plane P = { P1, N }
The reverse, to go from a point P1 and normal N to three points, you start from any direction G not along the normal N such that DOT(G,N)!=0. The two orthogonal directions along the plane are then
//try G={0,0,1} or {0,1,0} or {1,0,0}
G = {0,0,1};
if( MAG(CROSS(G,N))<TINY ) { G = {0,1,0}; }
if( MAG(CROSS(G,N))<TINY ) { G = {1,0,0}; }
U = UNIT( CROSS(N, G) );
V = CROSS(U,N);
P2 = P1 + U;
P3 = P1 + V;
A line is defined by a point and a direction. Typically two points (Q1, Q2) define the line
Q1 = {x1, y1, z1};
Q2 = {x2, y2, z2};
E = UNIT( Q2-Q1 );
Line L = { Q1, E }
The intersection of the line and plane are defined by the point on the line r=Q1+t*E that intersects the plane such that DOT(r-P1,N)=0. This is solved for the scalar distance t along the line as
t = DOT(P1-Q1,N)/DOT(E,N);
and the location as
r = Q1+(t*E);
NOTE: The DOT() returns the dot-product of two vector, CROSS() the cross-product, and UNIT() the unit vector (with magnitude=1).
DOT(P,Q) = P[0]*Q[0]+P[1]*Q[1]+P[2]*Q[2];
CROSS(P,Q) = { P[1]*Q[2]-P[2]*Q[1], P[2]*Q[0]-P[0]*Q[2], P[0]*Q[1]-P[1]*Q[0] };
UNIT(P) = {P[0]/sqrt(DOT(P,P)), P[1]/sqrt(DOT(P,P)), P[2]/sqrt(DOT(P,P))};
t*P = { t*P[0], t*P[1], t*P[2] };
MAG(P) = sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]);
One approach you may find easy to implement is to see where the plane intersects the coordinate axes. For the plane given by the equationaX + bY + cZ - d = 0 hold two variables at 0 and solve for the third. So the solutions would be (assuming a, b, c, and d are all non-zero):
(d/a, 0, 0)
(0, d/b, 0)
(0, 0, d/c)
You will need to consider the cases where one or more of the coefficients are 0 so you don't get a degenerate or colinear solutions. As an example if exactly one of the coefficients is 0 (say a=0) you instead use
(1, d/b, 0)
(0, d/b, 0)
(0, 0, d/c)
If exactly two of the coefficients are 0 (say a=0 and b=0) you can use:
(1, 0, d/c)
(0, 1, d/c)
(0, 0, d/c)
If d=0, the plane intersects the three axes at the origin, and so you can use:
(1, 0, -a/c)
(0, -c/b, 1)
(-b/a, 1, 0)
You will need to work out simular cases for d and exactly one other coefficient being 0, as well as d and two others being 0. There should be a total of 16 cases, but there are a few things that come to mind which should make that somewhat more manageable.
Where N=(Nx,Ny,Nz) is the normal, you could project the points N, (Ny,Nz,Nx), (Nz,Nx,Ny) onto the plane: they're guaranteed to be distinct.
Alternatively, if P and Q are on the plane, P+t(Q-P)xN is also on the plane for any t!=0 where x is the cross product.
Alternatively if M!=N is an arbitrary vector, K=MxN and L=KxN are colinear with the plane and any point p on the plane can be written as p=Origin+sK+tL for some s,t.