Implementing Collision Response in Simulation - c

I am attempting to implement collision response in a simulation that I am creating.
Basically, the program simulates a ball being thrown off a 50 meter building with some initial velocity.
I don't believe that the program is outputting realistic values for time of collision as well as values for x, y and vx, vy.
Here is the program:
#include<stdio.h>
#include<math.h>
#include<stdlib.h>
int main() {
FILE *fp;
FILE *fr;
//Declare and initialize all variables to be used
float ax = 0, ay = 0, x = 0, y = 0, vx = 0, vy = 0;
float time = 0, deltaTime = .001;
float vyImpact = 0, vxImpact = 0, xImpact = 0;
float old_y = 0, old_x = 0, old_vy = 0, old_vx = 0;
float deltaTime2 = 0, deltaTime3 = 0;
int numBounces = 0;
//Coefficient of Restitution; epsilon = ex = ey
float ex = .5;
float ey = .5;
fr = fopen("input_data.txt", "rt"); //Open file for reading
fp = fopen( "output_data.txt", "w" ); // Open file for writing
if(fr == NULL){ printf("File not found");} //if text file is not in directory...
if(fp == NULL){ printf("File not found");} //if text file is not in directory...
fscanf(fr, "ax: %f ay: %f x: %f y: %f vx: %f vy: %f\n", &ax, &ay, &x, &y, &vx, &vy);
while (numBounces < 9) {
//time = time + deltaTime
time = time + deltaTime;
//velocity[new] = velocity[old] + acc * deltaTime
vx = vx + ax*deltaTime;
vy = vy + ay*deltaTime;
//position[new] = position[old] + velocity*deltaTime + .5*acc*(deltaTime)^2
x = x + vx*deltaTime + (.5*ax*deltaTime*deltaTime);
y = y + vy*deltaTime + (.5*ay*deltaTime*deltaTime);
fprintf(fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t\n", ax, ay, x, y, vx, vy, time);
//Collision occurs; implement collision response
if (y < 0) {
//"Undo" values for y, x, and velocity
old_y = y - vy*deltaTime - (.5*ay*deltaTime*deltaTime);
old_x = x - vx*deltaTime - (.5*ax*deltaTime*deltaTime);
old_vy = vy - ay*deltaTime;
old_vx = vx - ax*deltaTime;
//Calculate time of collision
deltaTime2 = (-old_y + sqrt((old_y*old_y) - 2*ay*old_y)) / (ay);
printf("Time of Collision = %f\n", time - deltaTime2);
//Calculate velocity and x position at collsion
vyImpact = old_vy + ay*deltaTime2;
vxImpact = old_vx + ax*deltaTime2;
xImpact = old_x + old_vx*deltaTime2 + .5*ax*(deltaTime2*deltaTime2);
//Calculate new time for when ball bounces
deltaTime3 = deltaTime - deltaTime2;
//Calculate new x and y position and velocity for when ball bounces
x = xImpact + (ex)*vxImpact*deltaTime3 + .5*ax*(deltaTime3*deltaTime3);
y = 0 + (-ey)*vyImpact*deltaTime3 + .5*ay*(deltaTime3*deltaTime3);
vy = (-ey)*vyImpact + ay*deltaTime3;
vx = (ex)*vxImpact + ax*deltaTime3;
numBounces++;
printf("Number of Bounce(s) = %d\n", numBounces);
fprintf(fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t\n", ax, ay, x, y, vx, vy, time);
}
}
fclose(fp); //Close output file
fclose(fr); //Close input file
//system ("PAUSE");
return 0;
}
Basically, I am trying to produce accurate values so that I can see a plot of what this simulation is supposed to look like. I am assuming the logical errors have something to do with the physics. But being that my physics knowledge is limited, I am not able to see what exactly is wrong.
Here is sample input:
ax: 0 ay: -9.8 x: 0 y: 50 vx: 8.66 vy: 5

It seems to me that your problem may lie in how you're implementing the kinematics equations.
//velocity[new] = velocity[old] + acc * deltaTime
vx = vx + ax*deltaTime;
vy = vy + ay*deltaTime;
//position[new] = position[old] + velocity*deltaTime + .5*acc*(deltaTime)^2
x = x + vx*deltaTime + (.5*ax*deltaTime*deltaTime);
y = y + vy*deltaTime + (.5*ay*deltaTime*deltaTime);
Two things here: you're already taking the acceleration into account in your equations for vx and vy, and you're using summation rather than integrated equations. The .5*ax*deltaTime*deltaTime and .5*ay*deltaTime*deltaTime shouldn't be included. The equation x= 0.5*a*t^2 is used when calculating the distance traveled due to a constant acceleration for the total amount of time, based on the integral of the velocity equation. As you're doing summation and already include the acceleration in your velocity equations, there's no need to include the acceleration in the position equations.

Related

Programming Sine and Cosine in C more efficently

Im writing a C code programm that calcultates sine and cosine of a given angle without using the Sine and Cosine Functions of the Math.h library.
But the problem I am facing right now is that i can only calculate the sine and cosine of the Angles between -90° - 90° (so the angles in the first and fourth quadrant). The Cosine(100) = Cosine(80) with a negative operator. So my way of thinking would be to just write code that whenever it gets an angle that is greater than 90 and smaller than 270, it should just substract the additional value from 90; so in the case of Cos(240) that would be the same as Cos(90-150) with an inverted operator infront.
How should one go about this, without having to write 180-if statements?
#include <stdio.h>
#include <math.h>
int main() {
double alpha[29];
alpha[0] = 45.00000000;
alpha[1] = 26.56505118;
alpha[2] = 14.03624347;
alpha[3] = 7.12501635;
alpha[4] = 3.57633437;
alpha[5] = 1.78991061;
alpha[6] = 0.89517371;
alpha[7] = 0.44761417;
alpha[8] = 0.22381050;
alpha[9] = 0.11190568;
alpha[10] = 0.05595289;
alpha[11] = 0.02797645;
alpha[12] = 0.01398823;
alpha[13] = 0.00699411;
alpha[14] = 0.00349706;
alpha[15] = 0.00174853;
alpha[16] = 0.00087426;
alpha[17] = 0.00043713;
alpha[18] = 0.00021857;
alpha[19] = 0.00010928;
alpha[20] = 0.00005464;
alpha[21] = 0.00002732;
alpha[22] = 0.00001366;
alpha[23] = 0.00000683;
alpha[24] = 0.00000342;
alpha[25] = 0.00000171;
alpha[26] = 0.00000085;
alpha[27] = 0.00000043;
alpha[28] = 0.00000021;
double x = 0.60725294;
double y = 0;
double winkel = -150;
double theta = winkel;
double xs;
double ys;
int i = 0;
}
while ( i < 29 ){
printf("This is run number %d with theta = %lf \n", i, theta);
xs = y / pow(2, i);
ys = x / pow(2, i);
if (theta <= 0){
x = x + xs;
y = y - ys;
theta = theta + alpha[i];
} else {
x = x - xs;
y = y + ys;
theta = theta - alpha[i];
};
printf("x = %lf and y = %lf \n \n",x,y);
i++;
}
printf("cosine = %lf\n", x);
printf("sine = %lf\n", y);
return 0;
}
cos(x) = cos(-x)
cos(x) = cos(x%360) if x is in degrees and x is positive
those identities should be sufficient to understand what to do, right?
likewise sin(-x) = -sin(x)
sin(x) = sin(x%360) if x is in degrees and x is positive

Quaternion to euler angles and euler angles to quaternion in C

I have been writing this code for checking the euler angles and quaternions, but it is not run correcly (or maybe I do not understand the rotations):
#include <stdio.h>
#include <math.h>
#define DR2D (180 / M_PI)
#define DD2R (M_PI / 180)
int main(int argc, char** argv)
{
float x, y, z;
x = 0 * DD2R;
y = 0 * DD2R;
z = 180 * DD2R;
printf("x=%f y=%f z=%f\n", x, y, z);
float sx = sin(x / 2);
float sy = sin(y / 2);
float sz = sin(z / 2);
float cx = cos(x / 2);
float cy = cos(y / 2);
float cz = cos(z / 2);
float qx, qy, qz, qw;
printf("sx = %f sy = %f sz = %f cx = %f cy = %f cz = %f\n", sx, sy, sz, cx, cy, cy);
qx = cx*cy*sz + sx*sy*cz;
qy = sx*cy*cz + cx*sy*sz;
qz = cx*sy*cz - sx*cy*sz;
qw = cx*cy*cz - sx*sy*sz;
printf("Quaternion -> (%f, %f, %f, %f)\n", qx, qy , qz , qw);
//------------------------------------------------------------------
float sqw = qw*qw;
float sqx = qx*qx;
float sqy = qy*qy;
float sqz = qz*qz;
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
float test = qx*qy + qz*qw;
if (test > 0.499*unit) { // singularity at north pole
x = 2 * atan2(qx,qw);
y = M_PI/2;
z = 0;
}
else if (test < -0.499*unit) { // singularity at south pole
x = -2 * atan2(qx,qw);
y = -M_PI/2;
z = 0;
}
else {
x = atan2(2*qy*qw-2*qx*qz , sqx - sqy - sqz + sqw);
y = asin(2*test/unit);
z = atan2(2*qx*qw-2*qy*qz , -sqx + sqy - sqz + sqw);
}
printf("recover euler x=%.2f y=%.2f z=%.2f\n",
x * DR2D, y * DR2D, z * DR2D);
}
Because the output is very weird:
For example: x 180º y 90º z 90º
x=3.141593 y=1.570796 z=1.570796
sx = 1.000000 sy = 0.707107 sz = 0.707107 cx = -0.000000 cy = 0.707107 cz = 0.707107
Quaternion -> (0.500000, 0.500000, -0.500000, -0.500000)
reconversion euler x=270.00 y=90.00 z=0.00
Or for example x 90º y 90º z 90º
x=1.570796 y=1.570796 z=1.570796
sx = 0.707107 sy = 0.707107 sz = 0.707107 cx = 0.707107 cy = 0.707107 cz = 0.707107
Quaternion -> (0.707107, 0.707107, 0.000000, 0.000000)
recover euler x=180.00 y=90.00 z=0.00
The algorithm you use has a domain that lies in the interval [0,pi/2) only, the first quadrant. Or, because you want the input to be in degrees, between 0 (zero) inclusive and 90 degrees exclusive.

How to implement adaptive subdivision algorithm for curve in C

My homework is to write a C program with openGL/Glut which, after getting groups of 4 points by mouse click (points with 3 coordinates), should draw a bezier curve with adaptive algorithm. At a theoretical level it's clear how the algorithm works but I don't know how to put that in C code. I mean that at lesson we saw that the 4 control points could have a shape similar to a "trapeze" and then the algorithm calculates the two "heights" and then checks if they satisfy a tollerance. The problem is that the user might click everywhere in the screen and the points might not have trapeze-like shape...so, where can I start from? This is all I have
This is the cole I have written, which is called each time a control point is added:
if (bezierMode == CASTELJAU_ADAPTIVE) {
glColor3f (0.0f, 0.8f, 0.4f); /* draw adaptive casteljau curve in green */
for(i=0; i+3<numCV; i += 3)
adaptiveDeCasteljau3(CV, i, 0.01);
}
void adaptiveDeCasteljau3(float CV[MAX_CV][3], int position, float tolerance) {
float x01 = (CV[position][0] + CV[position+1][0]) / 2;
float y01 = (CV[position][1] + CV[position+1][1]) / 2;
float x12 = (CV[position+1][0] + CV[position+2][0]) / 2;
float y12 = (CV[position+1][1] + CV[position+2][1]) / 2;
float x23 = (CV[position+2][0] + CV[position+3][0]) / 2;
float y23 = (CV[position+2][1] + CV[position+3][1]) / 2;
float x012 = (x01 + x12) / 2;
float y012 = (y01 + y12) / 2;
float x123 = (x12 + x23) / 2;
float y123 = (y12 + y23) / 2;
float x0123 = (x012 + x123) / 2;
float y0123 = (y012 + y123) / 2;
float dx = CV[3][0] - CV[0][0];
float dy = CV[3][1] - CV[0][1];
float d2 = fabs(((CV[1][0] - CV[3][0]) * dy - (CV[1][1] - CV[3][1]) * dx));
float d3 = fabs(((CV[2][0] - CV[3][0]) * dy - (CV[2][1] - CV[3][1]) * dx));
if((d2 + d3)*(d2 + d3) < tolerance * (dx*dx + dy*dy)) {
glBegin(GL_LINE_STRIP);
glVertex2f(x0123, y0123);
glEnd();
return;
}
float tmpLEFT[4][3];
float tmpRIGHT[4][3];
tmpLEFT[0][0] = CV[0][0];
tmpLEFT[0][1] = CV[0][1];
tmpLEFT[1][0] = x01;
tmpLEFT[1][1] = y01;
tmpLEFT[2][0] = x012;
tmpLEFT[2][1] = y012;
tmpLEFT[3][0] = x0123;
tmpLEFT[3][1] = y0123;
tmpRIGHT[0][0] = x0123;
tmpRIGHT[0][1] = y0123;
tmpRIGHT[1][0] = x123;
tmpRIGHT[1][1] = y123;
tmpRIGHT[2][0] = x23;
tmpRIGHT[2][1] = y23;
tmpRIGHT[3][0] = CV[3][0];
tmpRIGHT[3][1] = CV[3][1];
adaptiveDeCasteljau3(tmpLEFT, 0, tolerance);
adaptiveDeCasteljau3(tmpRIGHT, 0, tolerance);
}
and obviously nothing is drawn. Do you have any idea?
the Begin / End should engulf your whole loop, not being inside for each isolated vertex !

Making projectile shoot using with allegro library in C

I'm trying to make a projectile shoot using with allegro library in C.And I couldn't do it in no way.My all code is below.My circle goes up but then disappear.Even if not I can't bring it down to the ground.I'm not good at physic so if my equals are wrong please forgive me.
#include <allegro.h>
#include < math.h >
void StartAlleg(); // my start program function
void EndAlleg();
int main() {
StartAlleg();
BITMAP *buffer = create_bitmap(640, 480);
int g = 10, Vo = 0 , Vx = 5, Vy = 475, angle= 0;
double time = 0, tUp = 0,hmax=0; //g is gravity
show_mouse(screen);
while (!key[KEY_ESC])
{
circle(buffer, Vx, Vy, 5, makecol(255, 0, 0));
if (key[KEY_UP]&&angle<360) angle++;
if (key[KEY_RIGHT]) Vo++;
if (key[KEY_DOWN] && angle>0) angle--;
if (key[KEY_LEFT] && Vo>0) Vo--;
textout_ex(buffer, font, "Player 1 : ", 0, 0, makecol(255, 255, 13), -1);
textprintf(buffer, font, 0, 25, makecol(255, 255, 13), "Angle = %d ",angle);
textprintf(buffer, font, 0, 15, makecol(255, 255, 13), "Speed = %d ", Vo);
if (key[KEY_Z] ){
Vx = Vo*cos(double(angle));
Vy = Vo*sin(double(angle));
if (angle== 180 || angle == 360) Vy = 0;
if (angle== 90 || angle== 270) Vx = 0;
if (Vx < 0) Vx *= (-1);
if (Vy < 0) Vy *= (-1);
tUp = Vy / g;
time = tUp * 2;
hmax = (Vy*Vy) / (2*g);
}
textprintf(buffer, font, 0, 35, makecol(255, 255, 13), "tUp Value = %.2f ", tUp);
for (int i = 1; i <= time; i++)
{
if (i<tUp){ Vx = Vx + g; Vy += g; }
else{ Vy -= g; Vx = Vx + g; }
}
blit(buffer, screen, 0, 0, 0, 0, 640, 480);
rest(60);
clear_bitmap(buffer);
}
EndAlleg(); // my end program function
return 0;
}
END_OF_MAIN()
void StartAlleg() {
int depth, res;
allegro_init();
depth = desktop_color_depth();
if (depth == 0) depth = 32;
set_color_depth(depth);
res = set_gfx_mode(GFX_AUTODETECT_WINDOWED, 640, 480, 0, 0);
if (res != 0) {
allegro_message(allegro_error);
exit(-1);
}
install_timer();
install_keyboard();
install_mouse();
install_sound(DIGI_AUTODETECT, MIDI_AUTODETECT, "A");
}
void EndAlleg() {
clear_keybuf();
}
I think , the main problem is here :
for (int i = 1; i <= time; i++)
{
if (i<tUp){ Vx = Vx + g; Vy += g; }
else{ Vy -= g; Vx = Vx + g; }
}
I didn't try to understand all your code, but it seems that your calculations are wrong.
Here is how gravity can be implemented :
First, you need to keep track of your projectile position, with variables like Px Py. This position will give you the drawing coordinates.
Then you need to keep track of its speed, usually horizontal and vertical speed, with variables like Vx Vy. If your initial speed is a single vector with angle, convert it once.
Every tick of your game (every loop iteration in your case), you add the speeds to the positions. Then to add gravity, you subtract 10 to the vertical speed, also at every tick (it implements acceleration of -10).
And thats all. Negative speeds and accelerations are normal, you don't need to check, but you can check for borders for positions. Also, you should note that you usually divide the speeds and accelerations by the frequency of your ticks, or else the faster your loop the faster the projectile will move.
You should note that this isn't the best way to implement gravity, because this only approximate physics (more ticks per second will give you more accurate simulation). You should google "game implement gravity properly" for an accurate algorithm, I'm not an expert.

Implementing Collision Detection

I wrote a program that will simulate a ball being thrown off a 50 meter building.
I added in collision detection by reversing the velocity in the y direction when the ball hits the ground (y < 0), keeping the horizontal velocity the same, and multiplying both velocities by some min value, so that the ball will ultimately come to a rest.
#include<stdio.h>
#include<math.h>
#include <stdlib.h>
int main() {
FILE *fp;
FILE *fr;
float ax = 0, ay = 0, x = 0, y = 0, vx = 0, vy = 0;
float time = 0, deltaTime = .001;
float min = -.00000000001;
int numBounces = 0;
fr = fopen("input_data.txt", "rt");
fp = fopen( "output_data.txt", "w" );
if(fr == NULL){ printf("File not found");}
if(fp == NULL){ printf("File not found");}
fscanf(fr, "ax: %f ay: %f x: %f y: %f vx: %f vy: %f\n", &ax, &ay, &x, &y, &vx, &vy);
while (vx > min && vy > min) {
time = time + deltaTime;
vx = vx + ax*deltaTime;
vy = vy + ay*deltaTime;
x = x + vx*deltaTime + (.5*ax*deltaTime*deltaTime);
y = y + vy*deltaTime + (.5*ay*deltaTime*deltaTime);
fprintf(fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t\n", ax, ay, x, y, vx, vy, time);
//Collision occurs; implement collision response
if(y < 0) {
vx = vx + ax*deltaTime*(.00001);
vy = -(vy + ay*deltaTime*(.00001));
numBounces++;
fprintf(fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t\n", ax, ay, x, y, vx, vy, time);
}
}
fclose(fp);
fclose(fr);
system ("PAUSE");
return 0;
}
I am not getting the correct values needed to produce a correct graph of the data.
It could be because my conditions in the while loop need to be changed, or that I did not implement collision response correctly.
Here is also some sample data:
ax: 0 ay: -9.8 x: 0 y: 50 vx: 8.66 vy: 5
for not outputing anything you can try fflush(fp) at the end of each cycle. and as far as I can see in your code your object gets some more speed whenever it hits the ground you have to change vy = -(vy + ay*deltaTime*(.00001)) to vy = -(vy - ay*deltaTime*(.00001)) to correct it. you can also create a better implementation for collision if you calculate the exact time of collision whenever y < 0 and then move object down, change speeds, and move object up for the rest of cycle to have more realistic collision.
we know that deltaY = 1/2*ay*t^2 + vy*t so we can compute t using the folling formula :
assuming py is the current height of object(it's distance to ground)
=> -py = 0.5 * ay* t * t + vy * t
=> 0 = 0.5 * ay * t * t+ vy * t + py
=> t = (-vy +- sqrt(vy*vy - 2 * ay * py)) / (2 * ay)
and since t has to be positive and knowing that ay is negetive and py is positive, we can assume the currect answer is
=> tc = (sqrt(vy*vy - 2 * ay * py) - vy) / 2 / ay
now we have tc which is time of collision. so we have to reverse the last changes in position and speed, then just step time tc seconds and then reverse vy and step deltaTime - tc seconds to complete that frame. so inside the if condition would be like (I just may have some problems doing the math, so if by any chance you didn't get expected results jsut doublecheck all equations):
if (y < 0) {
float tc = (sqrt(vy*vy - 2 *ay * y)) / 2 / ay;
x = x - vx*deltaTime - (.5*ax*deltaTime*deltaTime);
y = y - vy*deltaTime - (.5*ay*deltaTime*deltaTime);
vx = vx - ax * deltaTime;
vy = vy - ay * deltaTime;
vx = vx + ax * tc;
vy = vy + ay * tc;
x = x + vx*tc + (.5*ax*tc*tc);
y = y + vy*tc + (.5*ay*tc*tc);
vy = -(vy - ay*deltaTime*(.00001));
// you can also change above line and simply write
// vy = vy * -0.99;
// that will also create friction as you want it to be there
vx = vx + ax * (deltaTime - tc);
vy = vy + ay * (deltaTime - tc);
x = x + vx* (deltaTime - tc) + (.5*ax* (deltaTime - tc)* (deltaTime - tc));
y = y + vy* (deltaTime - tc) + (.5*ay* (deltaTime - tc)* (deltaTime - tc));
numBounces++;
fprintf(fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t\n", ax, ay, x, y, vx, vy, time);
}

Resources