I'm trying to write a program that bounces a square around the environment.
It should be changing it's velocity when it collides with the 'walls' and the central square.
I am fairly sure I am close to getting it right but there seems to be a real issue with the collision detection which I can't figure out.
Would greatly appreciate anyone having a look and giving some pointers.
fps = 40;
dt = 1/fps;
tmax = 10;
t = 0;
theta = 0;
dtheta = 1/20;
p = transpose([20,20,1]);
v = transpose([50,25,1]);
Vobj = transpose([2, -2, 1; 2,2,1; -2, 2, 1; -2, -2, 1]);
Vobj2 = transpose([60,60,1; 60,40,1; 40, 40, 1; 40, 60, 1]);
while t < tmax
x = p + t*v;
if x(1)<= 5 || x(1) >= 95
v(1)=-v(1);
elseif x(2)<= 5 || x(2) >= 95
v(2)=-v(2);
elseif ((40<=x(1)) && (x(1)<= 60)) && ((x(2) == 40) || (x(2) == 60))
v(2)=-v(2);
elseif ((40<=x(2)) && (x(2)<= 60)) && ((x(1) == 40) || (x(1) == 60))
v(1)=-v(1);
end
p = x;
% Transformations
T = [1,0,p(1);0,1,p(2);0,0,1];
S = [1+0.2*sin(2*t),0,0; 0,1+0.2*sin(2*t),0;0,0,1];
R = [cos(theta),sin(theta),0 ;-sin(theta),cos(theta),0;0,0,1];
L = R*S*T;
% Application of Transformations
V = L*Vobj;
fill(V(1,:),V(2,:),'r')
hold on
fill(Vobj2(1,:),Vobj2(2,:),'g')
axis([0,100,0,100])
hold off
shg;
pause(0.1);
% Updates
t = t + dt;
theta = theta + dtheta;
end
There are a couple things not entirely correct.
Your numerical integration should multiply with dt instead of t, if you are using a forward Euler method.
You are now checking for collision at the center of the moving block, but perhaps you actually want to check for collision of the corners of the block. If that's the case, you could loop over the coordinates of the corner of the block, and check them individually with the working collision detection if statement. So instead of checking once, do the check four times for the corners. Or even multiple times, if add points by interpolating the edges of the block.
The transformation of the block is incorrect. The location of V is not centered about the position p. If you try to simplify the problem, and just take the center of the block, you will see that the collision detection is in fact working.
fps = 40;
dt = 1/fps;
tmax = 10;
t = 0;
theta = 0;
dtheta = 1/20;
p = transpose([20,20,1]);
v = transpose([50,25,1]);
Vobj2 = transpose([60,60,1; 60,40,1; 40, 40, 1; 40, 60, 1]);
while t < tmax
x = p + dt*v;
if x(1)<= 5 || x(1) >= 95
v(1)=-v(1);
elseif x(2)<= 5 || x(2) >= 95
v(2)=-v(2);
elseif ((40<=x(1)) && (x(1)<= 60)) && ((x(2) == 40) || (x(2) == 60))
v(2)=-v(2);
elseif ((40<=x(2)) && (x(2)<= 60)) && ((x(1) == 40) || (x(1) == 60))
v(1)=-v(1);
end
p = x;
% plot the center of the block
scatter(p(1),p(2))
% plot the obstacle
hold on
fill(Vobj2(1,:),Vobj2(2,:),'g')
axis([0,100,0,100])
hold off
shg;
pause(0.1);
% Updates
t = t + dt;
theta = theta + dtheta;
end
Try to fix your transformation, by just applying some angle and translation, to a point where you know where your corners should end up (for example 90 degree rotation, or translation of a known value). This way you can check if the transformation is correct. Then incorporate this again in the simulation.