Rotate around a point other than origin? - c

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)

Related

Why is RK4 not updating values for my simulation of the Earth's orbit?

I am trying to use a Runge Kutta method to simulate the motion of the Earth around the Sun in C, I cannot see why but my code does not update the values for position or velocity and just keeps the initial values. The code I have written is:
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define dt 86400 // 1 day in seconds //
const double G = 6.67e-11;
const double au = 1.496e11;
const double M = 1.99e30;
double vx(double x, double y);
double vy(double x, double y);
double dx(double x, double y, double t);
double dy(double x, double y, double t);
double dvx(double x, double y, double t);
double dvy(double x, double y, double t);
int main(){
double initial_x = au;
double initial_y = 0;
double intiial_vx = vx(initial_x, initial_y);
double initial_vy = vy(initial_x, initial_y);
double t = 0;
double x = initial_x;
double y = initial_y;
double vx = initial_vx;
double vy = initial_vy;
for(int i=0;i<365;i++){
double k1x = dt(x,y,t);
double k2x = dt * dx(x + k1x/2, y + k1x/2, t + dt/2);
double k3x = dt * dx(x + k2x/2, y + k2x/2, t + dt/2);
double k4x = dt * dx(x + k3x, y + k3x, t + dt);
double kx = (1/6) * (k1x + 2*k2x + 2*k3x + k);
double k1y = dt * dy(x,y,t);
double k2y = dt * dy(x + k1y/2, y + k1y/2, t + dt/2);
double k3y = dt * dy(x + k2y/2, y + k2y/2, t + dt/2);
double k4y = dt * dy(x + k3y, y + k3y, t + dt);
double ky = (1/6) * (k1y + 2*k2y + 2*k3y + k4y);
double k1vx = dt * dvx(x,y,t);
double k2vx = dt * dvx(x+k1vx/2, y+k1vx/2, t + dt/2);
double k3vx = dt * dvx(x+k2vx/2, y+k2vx/2, t + dt/2);
double k4vx = dt * dvx(x+k3vx, y+k3vx, t+dt);
double kvx = (1/6) * (k1vx + 2*k2vx + 2*k3vx + k4vx);
double k1vy = dt * dvx(x,y,t);
double k2vy = dt * dvx(x+k1vy/2, y+k1vy/2, t + dt/2);
double k3vy = dt * dvx(x+k2vy/2, y+k2vy/2, t + dt/2);
double k4vy = dt * dvx(x+k3vy, y+k3vy, t+dt);
double kvy = (1/6) * (k1vy + 2*k2vy + 2*k3vy + k4vy);
x = x + kx;
y = y + ky;
vx = vx + kvx;
vy = vy + kvy;
printf("%.3e\t%.3e\t%.3e\t%.3e\n", x, y, vx, vy);
}
return 0;
}
// Function for the x velocity of a planet//
double vx(double x, double y)
{
double theta = atan(y/x);
double xVel = sqrt((G*M) / (sqrt(x*x + y*y))) * sin(theta);
return xVel;
}
// Function for the y velocity of a planet //
double vy(double x, double y)
{
double theta = atan(y/x);
double yVel = sqrt((G*M) / (sqrt(x*x + y*y))) * cos(theta);
return yVel;
}
// Function for dx //
double dx(double x, double y, double t)
{
double xVel = vx(x,y);
double dX = xVel*t;
return dX;
}
// Function for dy //
double dy(double x, double y, double t)
{
double yVel = vy(x,y);
double dY = yVel*t;
return dY;
}
// Function for dvx //
double dvx(double x, double y, double t)
{
double dVX = ((-G*M*x) / pow(x*x+y*y, 3/2)) * t;
return dVX;
}
// Function for dvy //
double dvy(double x, double y, double t)
{
double dVY = (((-G*M*x) / pow(x*x+y*y, 3/2))) * t;
return dVY;
}
I haven't yet added functions for the Runge-Kutta simply because I can't get it to work. Thanks for any help!
Your physics is wrong. The equations of motion do not explicitly depend on time. E.g, why dvx
double dVX = ((-G*M*x) / pow(x*x+y*y, 3/2)) * t;
should be different at different time? It shall only depend on x, y.
Do not multiply by t.
The Runge-Kutta implementation is wrong too. In your
double k1x = dt(x,y,t);
double k2x = dt * dx(x + k1x/2, y + k1x/2, t + dt/2);
double k3x = dt * dx(x + k2x/2, y + k2x/2, t + dt/2);
double k4x = dt * dx(x + k3x, y + k3x, t + dt);
double kx = (1/6) * (k1x + 2*k2x + 2*k3x + k);
you multiply by dt far too much. Keep in mind the dt is small. Every multiplication by a small value diminish the corrective power of subsequent k*x. You should do
double k1x = dx(x, y, t);
double k2x = dx(x + dt * k1x/x, y + dt * k1x/2, t + dt/2);
....
double kx = (dt/6) * (k1x + 2*k2x + 2*k3x + k4x);
// etc
Because you are calculating the k1vy's in terms of dvx. It should be dvy.
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
const int dt = 86400; // 1 day in seconds //
const double G = 6.67e-11;
const double au = 1.496e11;
const double M = 1.99e30;
double vx(double x, double y);
double vy(double x, double y);
double dx(double x, double y, double t);
double dy(double x, double y, double t);
double dvx(double x, double y, double t);
double dvy(double x, double y, double t);
int main(){
double initial_x = au;
double initial_y = 0;
double initial_vx = vx(initial_x, initial_y);
double initial_vy = vy(initial_x, initial_y);
double t = 0;
double x = initial_x;
double y = initial_y;
double vx = initial_vx;
double vy = initial_vy;
for(int i=0;i<365;i++){
double k1x = dt* dx(x,y,t);
double k2x = dt * dx(x + k1x/2, y + k1x/2, t + dt/2);
double k3x = dt * dx(x + k2x/2, y + k2x/2, t + dt/2);
double k4x = dt * dx(x + k3x, y + k3x, t + dt);
double kx = (1.0/6) * (k1x + 2*k2x + 2*k3x + k4x);
double k1y = dt * dy(x,y,t);
double k2y = dt * dy(x + k1y/2, y + k1y/2, t + dt/2);
double k3y = dt * dy(x + k2y/2, y + k2y/2, t + dt/2);
double k4y = dt * dy(x + k3y, y + k3y, t + dt);
double ky = (1.0/6) * (k1y + 2*k2y + 2*k3y + k4y);
double k1vx = dt * dvx(x,y,t);
double k2vx = dt * dvx(x+k1vx/2, y+k1vx/2, t + dt/2);
double k3vx = dt * dvx(x+k2vx/2, y+k2vx/2, t + dt/2);
double k4vx = dt * dvx(x+k3vx, y+k3vx, t+dt);
double kvx = (1.0/6) * (k1vx + 2*k2vx + 2*k3vx + k4vx);
double k1vy = dt * dvy(x,y,t);
double k2vy = dt * dvy(x+k1vy/2, y+k1vy/2, t + dt/2);
double k3vy = dt * dvy(x+k2vy/2, y+k2vy/2, t + dt/2);
double k4vy = dt * dvy(x+k3vy, y+k3vy, t+dt);
double kvy = (1.0/6) * (k1vy + 2*k2vy + 2*k3vy + k4vy);
x = x + kx;
y = y + ky;
vx = vx + kvx;
vy = vy + kvy;
printf("%.3e\t%.3e\t%.3e\t%.3e\n", x, y, vx, vy);
}
return 0;
}
// Function for the x velocity of a planet//
double vx(double x, double y)
{
double theta = atan(y/x);
double xVel = sqrt((G*M) / (sqrt(x*x + y*y))) * sin(theta);
return xVel;
}
// Function for the y velocity of a planet //
double vy(double x, double y)
{
double theta = atan(y/x);
double yVel = sqrt((G*M) / (sqrt(x*x + y*y))) * cos(theta);
return yVel;
}
// Function for dx //
double dx(double x, double y, double t)
{
double xVel = vx(x,y);
double dX = xVel*t;
return dX;
}
// Function for dy //
double dy(double x, double y, double t)
{
double yVel = vy(x,y);
double dY = yVel*t;
return dY;
}
// Function for dvx //
double dvx(double x, double y, double t)
{
double dVX = ((-G*M*x) / pow(x*x+y*y, 3/2)) * t;
return dVX;
}
// Function for dvy //
double dvy(double x, double y, double t)
{
double dVY = (((-G*M*x) / pow(x*x+y*y, 3/2))) * t;
return dVY;
}
Some other issues that you should check:
Take into account the fact that the planet's distance from the star will change over time
Your code does not check whether the planet's position is within the bounds of the simulation
Let's also look a little deeper at
double k2x = dt * dx(x + k1x/2, y + k1x/2, t + dt/2);
It should be obvious from the variable names that y + k1x/2 is wrong. It should be
y + k1y/2
Which then of course requires computing k1y before computing k2x.
You can only start computing the next stage of a Runge-Kutta method when you have completed the current stage in all components of the state vector.
By the way, the correct system is
x' = vx
y' = vy
vx' = ax(x,y)
vy' = ay(x,y)
so that the correct line for k2x would read
double k2x = dt * (vx + k1vx/2);
which of course again requires that k1vx be computed beforehand.
There is a big mistake here by updating x first, then y, then vx and finally vy because you have to make sure that values on the same stage are used for the evaluation of dvx and dvy.
I recommend re-arranging the terms to update x, y, vx and vy on each stage side by side.
There are numerous other mistakes and problems with your code. I'd rather show the correct way of doing this, rather than try to deal with all the bugs.
const double year = 365.0; // 1 year in days //
const double day = 86400.0; // 1 day in seconds //
const double G = 6.67e-11;
const double au = 1.496e11;
const double M = 1.99e30;
int main()
{
// Initial Conditions
double t = 0;
double x = au;
double y = 0;
double vx = init_vx(x, y);
double vy = init_vy(x, y);
const int steps = 365;
double dt = day * (year / steps);
// Initial Values
printf("%.3e\t%.3e\t%.3e\t%.3e\n", x, y, vx, vy);
// Simulation Loop
for (int i = 0; i < steps; i++)
{
// stage 1
double k1x = dx(vx, vy);
double k1y = dy(vx, vy);
double k1vx = dvx(x, y);
double k1vy = dvy(x, y);
//stage 2
double k2x = dx(vx + (dt / 2) * k1vx, vy + (dt / 2) * k1vy);
double k2y = dy(vx + (dt / 2) * k1vx, vy + (dt / 2) * k1vy);
double k2vx = dvx(x + (dt / 2) * k1x, y + (dt / 2) * k1y);
double k2vy = dvy(x + (dt / 2) * k1x, y + (dt / 2) * k1y);
//stage 3
double k3x = dx(vx + (dt / 2) * k2vx, vy + (dt / 2) * k2vy);
double k3y = dy(vx + (dt / 2) * k2vx, vy + (dt / 2) * k2vy);
double k3vx = dvx(x + (dt / 2) * k2x, y + (dt / 2) * k2y);
double k3vy = dvy(x + (dt / 2) * k2x, y + (dt / 2) * k2y);
//stage 4
double k4x = dx(vx + (dt) * k3vx, vy + (dt) * k3vy);
double k4y = dy(vx + (dt) * k3vx, vy + (dt) * k3vy);
double k4vx = dvx(x + (dt) * k3x, y + (dt) * k3y);
double k4vy = dvy(x + (dt) * k3x, y + (dt) * k3y);
// slope calculation
double kx = (k1x + (2.0 * k2x) + (2.0 * k3x) + k4x) / 6.0;
double ky = (k1y + (2.0 * k2y) + (2.0 * k3y) + k4y) / 6.0;
double kvx = (k1vx + (2.0 * k2vx) + (2.0 * k3vx) + k4vx) / 6.0;
double kvy = (k1vy + (2.0 * k2vy) + (2.0 * k3vy) + k4vy) / 6.0;
// integration step
t = t + dt;
x = x + dt * kx;
y = y + dt * ky;
vx = vx + dt * kvx;
vy = vy + dt * kvy;
// Simulation results
printf("%.3e\t%.3e\t%.3e\t%.3e\n", x, y, vx, vy);
}
return 0;
}
// Function for the initial x velocity of a planet//
double init_vx(double x, double y)
{
double theta = atan2(y, x);
double xVel = -sqrt((G * M) / (sqrt(x * x + y * y))) * sin(theta);
return xVel;
}
// Function for the initial y velocity of a planet //
double init_vy(double x, double y)
{
double theta = atan2(y, x);
double yVel = +sqrt((G * M) / (sqrt(x * x + y * y))) * cos(theta);
return yVel;
}
// Function for dx //
double dx(double vx, double vy)
{
double xVel = vx;
return xVel;
}
// Function for dy //
double dy(double vx, double vy)
{
double yVel = vy;
return yVel;
}
// Function for dvx //
double dvx(double x, double y)
{
double dVX = ((-G * M * x) / pow(x * x + y * y, 3 / 2.0));
return dVX;
}
// Function for dvy //
double dvy(double x, double y)
{
double dVY = ((-G * M * y / pow(x * x + y * y, 3 / 2.0)));
return dVY;
}
The code above has the flexibility to simulate 1 year in steps=365 or some other value. I would report out t/day values which is the time in days.
The big issues where
Incorrect handling of time in the d() functions.
Incorrect handling of intermediate values for each sub-step in RK4.
Careless use of integer division, 3/2 = 1 and not 1.5
Several typos all over the place, for example in dvy() it should use the y coordinate in the numerator.
Bug in initial conditions when (x,y) not int he first quadrant. Should use the atan2() function for finding the angle, and the x component should have a negative sign in front of it.
There are some optimizations that can be done to my code because some values are evaluated two times, but I think it won't make much of a difference as the floating point math is rather fast nowadays. I left them out for clarity really.

Converting Belge 1972 / Belgian Lambert 72 (EPSG:31370) coordinates to WGS 84 (EPSG:4326)

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);**

Finding Third Coordinate Of a Triangle

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).

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 !

Resources