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.
Related
I am trying to find a code that can convert Lambert72 coordinates to WGS84, so that I can get the same results as I get in this site, going to Menu APPs->Transform Coordinates.
https://mygeodata.cloud/cs2cs/
As an example I tried the following coordinates pair in this site, choosing on the left, the code 31370 (Lambert72) and on the right, WGS84.:
149334.41 167411
The result is:
4.35930680453; 50.817136997
For this result to work on Google Maps, the values must be switched, so the result I need is rather:
50.817136997; 4.35930680453
I tried the code from this answer in a similar post but I converted to C instead of C++/C#:
https://stackoverflow.com/a/40589076/1911497
So, my code, modified to C is this:
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
static void Lambert72toWGS84latlong(double X, double Y);
int main(){
double x, y;
x = 149334.41, y = 167411.0;
Lambert72toWGS84latlong(x, y);
return 0;
}
static void Lambert72toWGS84latlong(double X, double Y){
double LongRef = 0.076042943;
double nLamb = 0.7716421928;
double aCarre = pow(6378388.0,2.0);
double bLamb = 6378388.0 * (1.0 - (1.0 / 297.0));
double eCarre = (aCarre - pow(bLamb, 2.0)) / aCarre;
double KLamb = 11565915.812935;
double eLamb = sqrt(eCarre);
double eSur2 = eLamb / 2.0;
double Tan1 = (X - 150000.012) / (5400088.437 - Y);
double Lambda = LongRef + (1.0 / nLamb) * (0.000142043 + atan(Tan1));
double RLamb = sqrt(pow((X - 150000.012) , 2.0) + pow ((5400088.437 - Y) ,2.0));
double TanZDemi = pow((RLamb / KLamb),(1.0 / nLamb));
double Lati1 = 2.0 * atan(TanZDemi);
double eSin;
double Mult1, Mult2, Mult;
double LatiN, Diff;
double lat, lng ;
int i=0;
do{
eSin = eLamb * sin(Lati1);
Mult1 = 1.0 - eSin;
Mult2 = 1.0 + eSin;
Mult = pow((Mult1 / Mult2) , (eLamb / 2.0));
LatiN = (M_PI / 2.0) - (2.0 * (atan(TanZDemi * Mult)));
Diff = LatiN - Lati1;
printf("Diff: %d\n", abs(Diff));
//Lati1 = LatiN;
printf("Iterations: %d\n", i++);
}while (abs(Diff)> 0.0000000277777);
lat=LatiN;
lng=Lambda;
double SinLat = sin(lat);
double SinLng = sin(lng);
double CoSinLat = cos(lat);
double CoSinLng = cos(lng);
double dx = -125.8;
double dy = 79.9;
double dz = -100.5;
double da = -251.0;
double df = -0.000014192702;
double LWf = 1.0 / 297.0;
double LWa = 6378388.0;
double LWb = (1 - LWf) * LWa;
double LWe2 = (2.0 * LWf) - (LWf * LWf);
double Adb = 1.0 / (1.0 - LWf);
double Rn = LWa / sqrt(1.0 - LWe2 * SinLat * SinLat);
double Rm = LWa * (1 - LWe2) /pow((1.0 - LWe2 * lat * lat) ,1.5);
double DLat = -dx * SinLat * CoSinLng - dy * SinLat * SinLng + dz * CoSinLat;
DLat = DLat + da * (Rn * LWe2 * SinLat * CoSinLat) / LWa;
DLat = DLat + df * (Rm * Adb + Rn / Adb) * SinLat * CoSinLat;
DLat = DLat / (Rm + 0.0);
double DLng = (-dx * SinLng + dy * CoSinLng) / ((Rn + 0.0) * CoSinLat);
double Dh = dx * CoSinLat * CoSinLng + dy * CoSinLat * SinLng + dz * SinLat;
Dh = Dh - da * LWa / Rn + df * Rn * lat * lat / Adb;
double LatWGS84 = ((lat + DLat) * 180.0) / M_PI;
double LngWGS84 = ((lng + DLng) * 180.0) / M_PI;
printf("Latitude: %.8f\t Longitude: %.8f\n", LatWGS84, LngWGS84);
}
However I don't get the same result.
The results I get are:
Latitude: 50.78274764 Longitude: 4.35930689
What would I need to change to be able to get the same result as the one returned by the mentioned site above?
Thanks
PsySc0rpi0n
do {
// stuff
} while (abs(Diff)> 0.0000000277777);
// **int abs(int);**
You probably want
do {
// stuff
} while (fabs(Diff) > 0.0000000277777);
// **double fabs(double);**
i have an assignment for Programming class.
i have finished 80 percent of it, i am not able to complete the last part.
We have to find the distance between the shore and the boat(Point C) And Coordinates Of The Boat (Point C).
We are given (X,Y) coordinates And Angles of Points A and B.
Given A(76,316) B(57,516) Angle A = 17° and B = 17° Angle.
So Far i found AB= 200.9 AC= 105.04 BC = 105.04 C = 146° And the distance between the shore and the boat d = 30.71.
I tried everything but i am not able to find the Coordinates of Point C (Boat).
The Output is also Given : d = 30.71, Coordinates of C = C(97.07,418.90)
Please help me to find coordinates of Point C.
#include <stdio.h>
#include <math.h>
float radian(int degree);
float get_watchtowers_distance(int x1,int y1,int x2,int y2);
float get_boat_distance(float d,int alpha,int beta);
int main(){
get_watchtowers_distance(76,316,57,516);
get_boat_distance(200.90,17,17);
}
float radian(int degree){
return degree * (M_PI/180);
}
float get_watchtowers_distance(int x1,int y1,int x2,int y2){
return sqrtf(powf(x2-x1,2.0f)+powf(y2-y1,2.0f));
}
float get_boat_distance(float d,int alpha,int beta){
float a,b,c = 180 - (float)alpha - (float)beta;
a = (float)radian(alpha);
b = (float)radian(beta);
c = (float)radian(c);
printf("%.2f %.2f %.2f\n",a,b,c);
float B,A = d*sinf(a)/sinf(c);
B = d*sinf(b)/sinf(c);
float dist,area = 1.0f/2.0f*A*B*sinf(c);
dist = 2*area/d;
printf("AC Distance : %.2f , BC distance : %.2f\n",A,B);
printf("Boat Distance : %.2f\n", dist);
return dist;
}
Simpler approach to find d distance. We can express lengths of the left and right parts of base through d and cotangents
aa = d * ctg(alpha)
bb = d * ctg(beta)
aa + bb = l
d * (ctg(alpha) + ctg(beta)) = l
d = l / (ctg(alpha) + ctg(beta)) =
= l * sin(alpha) * sin(beta) / sin(alpha+beta)
Now you can find C coordinates using normalized vector ab and perpendicular component
abx = (b.x - a.x) / l
aby = (b.y - a.y) / l
aalen = d / tg(alpha)
c.x = a.x + abx * aalen - aby * d
c.y = a.y + aby * aalen + abx * d
There you go I have written some code for your question..
float get_coordinates(int x1, int y1, int x2, int y2, float alpha, float beta){
float a_angle_alpha, b_angle_beta, c_angle_delta;
c_angle_delta = 180 - (float)alpha - (float)beta;
printf("Alpha Angle in Degrees = %.2f\nBeta Angle in Degrees = %.2f\nDelta Angle in Degrees = %.2f\n", alpha, beta, c_angle_delta);
a_angle_alpha = (float)radian(alpha);
b_angle_beta = (float)radian(beta);
c_angle_delta = (float)radian(c_angle_delta);
printf("Alpha in radiance: %.2f\nBeta in radiance: %.2f\nDelta in radiance: %.2f\n", a_angle_alpha, b_angle_beta, c_angle_delta);
int u, v;
float d;
u = x2 - x1;
v = y2 -y1;
printf("u = %.d\nv = %.d\n", u, v);
float d1_distance_AB, d2_distance_BC, d3_distance_AC;
d1_distance_AB = sqrtf((u*u)+ (v*v));
d2_distance_BC = d1_distance_AB*sinf(b_angle_beta)/sinf(c_angle_delta);
d3_distance_AC = d1_distance_AB*sinf(a_angle_alpha)/sinf(c_angle_delta);
printf("Distance between A and B: %.2f\nDistance between B and C: %.2f\nDistance between A and C: %.2f\n",d1_distance_AB, d2_distance_BC, d3_distance_AC);
float RHS1, RHS2;
float x3, y3;
RHS1 = ((x1 * u) + (y1 * v) + (d2_distance_BC * d1_distance_AB * cosf(a_angle_alpha)));
RHS2 = ((y2 * u) - (x2 * v) - (d2_distance_BC * d1_distance_AB * sinf(a_angle_alpha)));
x3 = ((1 / (d1_distance_AB * d1_distance_AB)) * ((u * RHS1) - (v * RHS2)));
y3 = ((1 / (d1_distance_AB * d1_distance_AB)) * ((v * RHS1) + (u * RHS2)));
printf("Coordinates of Boat at point C is = %.2f:%.2f (x:y)", x3, y3);
float coordinates[2] = {x3,y3};
return coordinates;
}
Full Code:
#include <stdio.h>
#include <math.h>
float radian(int degree);
float get_watchtowers_distance(int x1,int y1,int x2,int y2);
float get_boat_distance(float d,int alpha,int beta);
float get_coordinates(int x1, int y1, int x2, int y2, float alpha, float beta);
int main(){
get_watchtowers_distance(76,316,57,516);
get_boat_distance(200.90,17,17);
get_coordinates(76,316,57,516,17,17);
}
float radian(int degree){
return degree * (M_PI/180);
}
float get_watchtowers_distance(int x1,int y1,int x2,int y2){
return sqrtf(powf(x2-x1,2.0f)+powf(y2-y1,2.0f));
}
float get_boat_distance(float d,int alpha,int beta){
float a,b,c = 180 - (float)alpha - (float)beta;
a = (float)radian(alpha);
b = (float)radian(beta);
c = (float)radian(c);
printf("%.2f %.2f %.2f\n", a, b, c);
float B,A = d*sinf(a)/sinf(c);
B = d*sinf(b)/sinf(c);
float dist,area = 1.0f/2.0f*A*B*sinf(c);
dist = 2*area/d;
printf("AC Distance : %.2f , BC distance : %.2f\n",A,B);
printf("Boat Distance : %.2f\n", dist);
return dist;
}
float get_coordinates(int x1, int y1, int x2, int y2, float alpha, float beta){
float a_angle_alpha, b_angle_beta, c_angle_delta;
c_angle_delta = 180 - (float)alpha - (float)beta;
printf("Alpha Angle in Degrees = %.2f\nBeta Angle in Degrees = %.2f\nDelta Angle in Degrees = %.2f\n", alpha, beta, c_angle_delta);
a_angle_alpha = (float)radian(alpha);
b_angle_beta = (float)radian(beta);
c_angle_delta = (float)radian(c_angle_delta);
printf("Alpha in radiance: %.2f\nBeta in radiance: %.2f\nDelta in radiance: %.2f\n", a_angle_alpha, b_angle_beta, c_angle_delta);
int u, v;
float d;
u = x2 - x1;
v = y2 -y1;
printf("u = %.d\nv = %.d\n", u, v);
float d1_distance_AB, d2_distance_BC, d3_distance_AC;
d1_distance_AB = sqrtf((u*u)+ (v*v));
d2_distance_BC = d1_distance_AB*sinf(b_angle_beta)/sinf(c_angle_delta);
d3_distance_AC = d1_distance_AB*sinf(a_angle_alpha)/sinf(c_angle_delta);
printf("Distance between A and B: %.2f\nDistance between B and C: %.2f\nDistance between A and C: %.2f\n",d1_distance_AB, d2_distance_BC, d3_distance_AC);
float RHS1, RHS2;
float x3, y3;
RHS1 = ((x1 * u) + (y1 * v) + (d2_distance_BC * d1_distance_AB * cosf(a_angle_alpha)));
RHS2 = ((y2 * u) - (x2 * v) - (d2_distance_BC * d1_distance_AB * sinf(a_angle_alpha)));
x3 = ((1 / (d1_distance_AB * d1_distance_AB)) * ((u * RHS1) - (v * RHS2)));
y3 = ((1 / (d1_distance_AB * d1_distance_AB)) * ((v * RHS1) + (u * RHS2)));
printf("Coordinates of Boat at point C is = %.2f:%.2f (x:y)", x3, y3);
float coordinates[2] = {x3,y3};
return coordinates;
}
Output:
0.30 0.30 2.55
AC Distance : 105.04 , BC distance : 105.04
Boat Distance : 30.71
Alpha Angle in Degrees = 17.00
Beta Angle in Degrees = 17.00
Delta Angle in Degrees = 146.00
u = -19
v = 200
Distance between A and B: 200.90
Distance between B and C: 105.04
Distance between A and C: 105.04
Coordinates of Boat at point C is = 97.07:418.90 (x:y)
To find the distance d directly without determining point C, first, you can calculate the area S of the triangle in two ways.
S = 0.5 * l * d
S = 0.5 * l * l * sin(a) * sin(b) / sin(a + b)
Equating the two and canceling out 0.5 * l gives d = l * sin(a) * sin(b) / sin(a + b).
This is more math-y than programming-y but how can I improve this rotation matrix to accept a point at which to rotate around? There's the axis, which specifies the nature of the rotation, but there's also the point at which the vertices are rotated around per the axis. How can I improve this matrix to accept a position? It happens to be written in C but that's not very relevant, because I'm looking for the logic.
matrix_float4x4 Rotate(const float Radians, const float X, const float Y, const float Z) {
const float
Sin = sinf(Radians),
Cos = cosf(Radians),
C = 1-Cos, SinX = Sin*X,
SinY = Sin*Y,
SinZ = Sin*Z,
CX = X*C,
CY = Y*C,
CZ = C*Z;
return (matrix_float4x4){
.columns = {
{Cos+(X*CX), (Y*CX)+SinZ, (Z*CX)-SinY, 0},
{(X*CY)-SinZ, Cos+(Y*CY), (Z*CY)+SinX, 0},
{(X*CZ)+SinY, (Y*CZ)-SinX, Cos+(Z*CZ), 0},
{0, 0, 0, 1}
}
};
}
The question is, do you want to transform the object or the world? In other words, you can rotate the object and move (translate) it to a specific position or you can rotate the world around a specific point (orbit camera).
As pointed out by 'scg': T * R != R * T
A Transformation formula for the object would be: T * C * R * -C
(T: Translate, C: Center, R: Rotation)
Now, there are a lot of operations involved. If you look at a Translation- or Rotationmatrix, there are a lot of zeros. You can eliminate those steps:
Example:
typedef struct mat4_rec {
float
a11, a12, a13, a14,
a21, a22, a23, a24,
a31, a32, a33, a34,
a41, a42, a43, a44;
} mat4_t;
mat4_t* translate(mat4_t *m, float x, float y, float z)
{
m->a14 += m->a11 * x + m->a12 * y + m->a13 * z;
m->a24 += m->a21 * x + m->a22 * y + m->a23 * z;
m->a34 += m->a31 * x + m->a32 * y + m->a33 * z;
m->a44 += m->a41 * x + m->a42 * y + m->a43 * z;
return m;
}
To translate a 4x4 Matrix (M *= T). These are a lot less operations than a full 4x4 Multiplication.
Rotation (M *= R) would look like this:
mat4_t* rotate(mat4_t *m, float rad, float x, float y, float z)
{
float s = sinf(rad);
float c = cosf(rad);
float t = 1.0f - c;
float a11 = x * x * t + c;
float a12 = x * y * t - z * s;
float a13 = x * z * t + y * s;
float a21 = y * x * t + z * s;
float a22 = y * y * t + c;
float a23 = y * z * t - x * s;
float a31 = z * x * t - y * s;
float a32 = z * y * t + x * s;
float a33 = z * z * t + c;
float m11 = m->a11;
float m12 = m->a12;
float m13 = m->a13;
float m21 = m->a21;
float m22 = m->a22;
float m23 = m->a23;
float m31 = m->a31;
float m32 = m->a32;
float m33 = m->a33;
float m41 = m->a41;
float m42 = m->a42;
float m43 = m->a43;
m->a11 = m11 * a11 + m12 * a21 + m13 * a31;
m->a12 = m11 * a12 + m12 * a22 + m13 * a32;
m->a13 = m11 * a13 + m12 * a23 + m13 * a33;
m->a21 = m21 * a11 + m22 * a21 + m23 * a31;
m->a22 = m21 * a12 + m22 * a22 + m23 * a32;
m->a23 = m21 * a13 + m22 * a23 + m23 * a33;
m->a31 = m31 * a11 + m32 * a21 + m33 * a31;
m->a32 = m31 * a12 + m32 * a22 + m33 * a32;
m->a33 = m31 * a13 + m32 * a23 + m33 * a33;
m->a41 = m41 * a11 + m42 * a21 + m43 * a31;
m->a42 = m41 * a12 + m42 * a22 + m43 * a32;
m->a43 = m41 * a13 + m42 * a23 + m43 * a33;
return m;
}
To implement the transformation: T * C * R * -C
mat4_t m; // <- set to identiy or something else
// T - move position of object by one unit along the x axis
translate(&m, 1, 0, 0);
// C - move pivot point down along the y axis
translate(&m, 0, -1, 0);
// R - 180 degress around the z axis
rotate(&m, M_PI, 0, 0, 1);
// -C - restore pivot point
translate(&m, 0, 1, 0);
// multiply matrix by any vector (M * V)
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);
}
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.