bool line_segments_intersect(vector_float2 p1, vector_float2 p2, vector_float2 q1, vector_float2 q2)
{
vector_float2 r = {p2.x - p1.x, p2.y - p1.y};
vector_float2 s = {q2.x - q1.x, q2.y - q1.y};
vector_float2 q_minus_p = {q1.x - p1.x, q1.y - p1.y};
float r_cross_s = vector_cross(r, s).z;
float q_minus_p_cross_r = vector_cross(q_minus_p, r).z;
float q_minus_p_cross_s = vector_cross(q_minus_p, s).z;
if (q_minus_p_cross_r == 0 && r_cross_s == 0) {
// The lines are colinear
float magnitude_r = vector_length(r);
float s_dot_r = vector_dot(s, r);
float t0 = vector_dot(q_minus_p, r) / magnitude_r;
float t1 = t0 + s_dot_r / magnitude_r;
return ((t0 >= 0 && t0 <= 1) || (t1 >= 0 && t1 <= 1));
} else if (r_cross_s == 0 && q_minus_p_cross_r != 0) {
// The lines are parallel and non-intersecting
return false;
} else if (r_cross_s != 0) {
float t = q_minus_p_cross_s / r_cross_s;
float u = q_minus_p_cross_r / r_cross_s;
// Normally you'd want to test for 0 <= t <= 1 && 0 <= u <= 1, but
// that would mean that two lines that share the same endpoint are
// marked as intersecting, which isn't what we want for our use case.
return t > 0 && t < 1 && u > 0 && u < 1;
}
return false;
}
/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
float LSM303_heading()
{
vector_float from = {0,0,1}; //Z axis forward
vector_float temp_m = {LSM303_m.x, LSM303_m.y, LSM303_m.z};
vector_float temp_a = {LSM303_a.x, LSM303_a.y, LSM303_a.z};
// subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((float)m_min.x + m_max.x) / 2;
temp_m.y -= ((float)m_min.y + m_max.y) / 2;
temp_m.z -= ((float)m_min.z + m_max.z) / 2;
// compute E and N
vector_float E;
vector_float N;
vector_cross(&temp_m, &temp_a, &E);
vector_normalize(&E);
vector_cross(&temp_a, &E, &N);
vector_normalize(&N);
// compute heading
float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
DEBUGPRINT("Heading: %0.1f\n", heading);
return heading;
}
END_TEST
START_TEST(test_vector_three) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
double A[3], B[3], C[3], tmp[3];
double D, E, F, norm;
for (i = 0; i < 3; i++) {
A[i] = mrand / 1e20;
B[i] = mrand / 1e20;
C[i] = mrand / 1e20;
}
/* Check triple product identity */
vector_cross(A, B, tmp);
D = vector_dot(3, C, tmp);
vector_cross(B, C, tmp);
E = vector_dot(3, A, tmp);
vector_cross(C, A, tmp);
F = vector_dot(3, B, tmp);
norm = (vector_norm(3, A) + vector_norm(3, B) + vector_norm(3, C))/3;
fail_unless(fabs(E - D) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
D, E);
fail_unless(fabs(E - F) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
E, F);
fail_unless(fabs(F - D) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
F, D);
}
}
//3D viewing pipeline. VTM is complete view matrix. none of the values of the View structure should be edited.
void matrix_setView3D(Matrix *vtm, View3D *view){
Vector u, vup, vpn;
double b, d;
matrix_identity(vtm);
matrix_translate(vtm, -view->vrp.val[0], -view->vrp.val[1], -view->vrp.val[2]);
vpn = view->vpn;
vector_cross(&view->vup, &vpn, &u);
vector_cross(&vpn, &u, &vup);
vector_normalize(&u);
vector_normalize(&vup);
vector_normalize(&vpn);
matrix_rotateXYZ(vtm, &u, &vup, &vpn);
matrix_translate(vtm, 0.0, 0.0, view->d);
// in lecture notes here (6 and 7) it says to shear but as we only have d to define the COP I don't think we have to
b = view->d + view->b;
matrix_scale(vtm, ((2.0*view->d) / (b*view->du)), ((2.0*view->d) / (b*view->dv)), (1/b));
d = view->d / b;
matrix_perspective(vtm, d);
matrix_scale2D(vtm, (-view->screenx / (2.0*d)), (-view->screeny / (2.0*d)));
matrix_translate2D(vtm, (view->screenx / 2.0), (view->screeny / 2.0));
}
END_TEST
START_TEST(test_vector_cross) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
double A[3], B[3], C[3], D[3];
for (i = 0; i < 3; i++) {
A[i] = mrand;
B[i] = A[i];
}
vector_cross(A, B, C);
for (i = 0; i < 3; i++)
fail_unless(fabs(C[i]) < LINALG_TOL,
"Vector element differs from 0: %lf",
C[i]);
for (i = 0; i < 3; i++) {
A[i] = mrand;
B[i] = mrand;
}
vector_cross(A, B, C);
for (i = 0; i < 3; i++) B[i] *= -1;
vector_cross(B, A, D);
for (i = 0; i < 3; i++)
fail_unless(fabs(C[i] - D[i]) < LINALG_TOL,
"Vector equality fails: %lf != %lf",
C[i], D[i]);
}
}
// Calculate the heading
float calcHeading(float *from, const Accelerometer *acc, const Magnetometer *mag)
{
// Change values with values from calibration tests...
int16_t min[] = { 32767, 32767, 32767 };
int16_t max[] = { -32767, -32767, -32767 };
float temp_m[] = { mag->x, mag->y, mag->z };
float temp_a[] = { acc->x, acc->y, acc->z };
// Initialize east and north vector
float east[] = {0, 0, 0};
float north[] = {0, 0, 0};
int i;
for(i = 0; i < 3; i ++)
temp_m[i] -= (min[i]+max[i])/2;
// Calculate North and East vectors
vector_cross(temp_m, temp_a, east);
vector_normalize(east);
vector_cross(temp_a, east, north);
vector_normalize(north);
// Calculate angular difference
float heading = atan2(vector_dot(east, from), vector_dot(north,from))*180/M_PI;
if (heading < 0)
heading += 360;
return heading;
}
// Returns the angular difference in the horizontal plane between the
// From vector and North, in degrees.
//
// Description of heading algorithm:
// Shift and scale the magnetic reading based on calibration data to
// to find the North vector. Use the acceleration readings to
// determine the Up vector (gravity is measured as an upward
// acceleration). The cross product of North and Up vectors is East.
// The vectors East and North form a basis for the horizontal plane.
// The From vector is projected into the horizontal plane and the
// angle between the projected vector and north is returned.
int LSM303::heading(vector from)
{
// shift and scale
m_dataMag.x = (m_dataMag.x - m_minMag.x) / (m_maxMag.x - m_minMag.x) * 2 - 1.0;
m_dataMag.y = (m_dataMag.y - m_minMag.y) / (m_maxMag.y - m_minMag.y) * 2 - 1.0;
m_dataMag.z = (m_dataMag.z - m_minMag.z) / (m_maxMag.z - m_minMag.z) * 2 - 1.0;
vector temp_a(m_dataAcc);
vector temp_m(m_dataMag);
// normalize
vector_normalize(temp_a);
//vector_normalize(&m);
// compute E and N
vector E;
vector N;
vector_cross(temp_m, temp_a, E);
vector_normalize(E);
vector_cross(temp_a, E, N);
// compute heading
int heading = round(atan2(vector_dot(E, from), vector_dot(N, from)) * 180 / M_PI);
if (heading < 0) heading += 360;
return heading;
}
/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
float Compass::getHeading()
{
vector<int> from = (vector<int>){0, -1, 0};
compass->read();
vector<int32_t> temp_m = {compass->magData.x, compass->magData.y, compass->magData.z};
vector<int32_t> temp_a = {compass->accelData.x, compass->accelData.y, compass->accelData.z};
// subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((int32_t)calMin.x + calMax.x) / 2;
temp_m.y -= ((int32_t)calMin.y + calMax.y) / 2;
temp_m.z -= ((int32_t)calMin.z + calMax.z) / 2;
// compute E and N
vector<float> E;
vector<float> N;
vector_cross(&temp_m, &temp_a, &E);
vector_normalize(&E);
vector_cross(&temp_a, &E, &N);
vector_normalize(&N);
// compute heading
float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * RAD2DEG;
// correcting for mounting direction
heading -= 90.0;
if (heading < 0) heading += 360;
return heading;
}
void matrix_setView3D(Matrix *vtm, View3D *view){
if(NULL != vtm && NULL != view){
Vector u;
Vector vup = view->vup;
Vector vpn = view->vpn;
Matrix project;
double bPrime = view->d +view->b;
double dPrime = view->d/bPrime;
matrix_identity(vtm);
printf("before everything:\n");
matrix_print(vtm, stdout);
vector_cross(&vup,&vpn,&u);
vector_cross(&vpn,&u,&vup);
printf("vrp:\n");
vector_print(&view->vrp,stdout);
matrix_translate(vtm, -view->vrp.val[0], -view->vrp.val[1],-view->vrp.val[2]);
printf("After VRP translation:\n");
matrix_print(vtm, stdout);
vector_normalize(&u);
vector_normalize(&vpn);
vector_normalize(&vup);
matrix_rotateXYZ(vtm, &u, &vup, &vpn );
printf("After Rxyz :\n");
matrix_print(vtm, stdout);
matrix_translate(vtm, 0, 0,view->d);
printf("After translating COP to origin:\n");
matrix_print(vtm, stdout);
matrix_scale(vtm, (2*view->d)/(bPrime*view->du), (2*view->d)/(bPrime*view->dv), 1/bPrime);
printf("After scaling to CVV:\n");
matrix_print(vtm, stdout);
matrix_identity(&project);
project.m[3][3]=0;
project.m[3][2]=1/dPrime;
printf("projection:\n");
matrix_print(&project, stdout);
matrix_multiply(&project,vtm,vtm);
printf("After perspective:\n");
matrix_print(vtm, stdout);
matrix_scale2D(vtm, -view->screenx/(2*dPrime), -view->screeny/(2*dPrime));
printf("After scale to image coords:\n");
matrix_print(vtm, stdout);
matrix_translate2D(vtm, view->screenx/2, view->screeny/2);
printf("After final translation to image coords:\n");
matrix_print(vtm, stdout);
}
}
t_point cylinder_normal(t_list *list, t_point point)
{
t_point normal;
t_point tmp;
tmp = vector_cross(vector_direc(list->pos, point), (t_point){0, 0, 1});
normal = vector_cross(tmp, (t_point){0, 0, -1});
normal = norm(normal);
return (normal);
}
camera init_camera(point3 position, point3 at, vec3 up, float fov, float aspect) {
camera cam;
cam.fov = fov;
cam.aspect = aspect;
cam.position = position;
cam.zdir = vector_normalized(vector_minus(at, position));
cam.xdir = vector_normalized(vector_cross(up, cam.zdir));
cam.ydir = vector_normalized(vector_cross(cam.zdir, cam.xdir));
cam.center = vector_float_mul(1.f / tanf((cam.fov * M_PI / 180.f) * 0.5f), cam.zdir);
return cam;
}
vector3d ahrs_drift_correction(dataexchange_t *data) {
vector3d corr_vector, acc_g;
corr_vector.x = 0.0;
corr_vector.y = 0.0;
corr_vector.z = 0.0;
//do correction only then acceleration is close to 1G (unreliable if greater)
acc_g = adxl345_raw_to_g(data, SCALE_2G_10B);
double acc_magnitude = vector_magnitude(acc_g);
if (fabs(acc_magnitude - 1.0) <= 0.15) {
float corr_strength = 0.15;
//vectors for rotation matrix
vector3d acc_v, mag_v;
acc_v.x = (*data).acc_x;
acc_v.y = (*data).acc_y;
acc_v.z = (*data).acc_z;
mag_v.x = (*data).mag_x;
mag_v.y = (*data).mag_y;
mag_v.z = (*data).mag_z;
hmc5883l_applyCalibration(&mag_v, calib_ptr);
vector3d down = vector_inv(acc_v);
vector3d east = vector_cross(down, mag_v);
vector3d north = vector_cross(east, down);
//normalize vectors
vector_norm(&down);
vector_norm(&east);
vector_norm(&north);
//matrix from rotation quaternion
matrix3x3d rot_matrix = quaternion_to_matrix((*data).qr);
//correction vector calculation
vector3d sum1 = vector_sum(vector_cross(north, matrix_row_to_vector(&rot_matrix, 1)),
vector_cross(east, matrix_row_to_vector(&rot_matrix, 2)));
vector3d sum2 = vector_sum(sum1, vector_cross(down, matrix_row_to_vector(&rot_matrix, 3)));
corr_vector = vector_scale(sum2, corr_strength);
}
return corr_vector;
}
static void
plane_normal(plane p, vector *n)
{
vector v1, v2;
vector_subtract(p.p1, p.p2, &v1);
vector_subtract(p.p1, p.p3, &v2);
vector_cross(v2, v1, n);
}
int get_heading (data_t* data) {
// Initialize heading value - it's an integer because it is
// going to eventually be binned into North, South, East, or West
vector north, east, mag_vector, accel_vector, from;
int heading;
// Initialize north and east vectors
vector_init(&north, 0, 0, 0);
vector_init(&east, 0, 0, 0);
// Put magnetometer and accelerometer data into vector format
vector_init(&mag_vector, data->Mx, data->My, data-> Mz);
vector_init(&accel_vector, data->Ax, data->Ay, data-> Az);
// Initialize "from" vector based on carrier board design
vector_init(&from, 1, 0, 0);
// Subtract calibration offset from magnetometer readings
mag_vector.x -= ((float) MAG_MAX_X + MAG_MIN_X) / 2.0;
mag_vector.y -= ((float) MAG_MAX_Y + MAG_MIN_Y) / 2.0;
mag_vector.z -= ((float) MAG_MAX_Z + MAG_MIN_Z) / 2.0;
// Compute East and North
vector_cross(&mag_vector, &accel_vector, &east);
vector_norm(&east);
vector_cross(&accel_vector, &east, &north);
vector_norm(&north);
// Compute the heading and make sure it's positive
heading = (int)(atan2(vector_dot(&east, &from), vector_dot(&north, &from))* 180 / PI);
if (heading < 0) {
heading += 360;
}
// Memory cleanup
vector_destroy(&north);
vector_destroy(&east);
vector_destroy(&mag_vector);
vector_destroy(&accel_vector);
vector_destroy(&from);
return heading;
}
static void compute_normal(point_t* points, int num_points, vector_t* n)
{
vector_t A, B;
point_displacement(&points[0], &points[1], &A);
point_displacement(&points[0], &points[2], &B);
vector_cross(&A, &B, n);
ASSERT(vector_mag(n) > 0.0);
vector_normalize(n);
}
t_point plan_normal(t_list *list, t_point point)
{
t_point normal;
(void)point;
normal = vector_cross(list->ori2, list->ori);
normal = norm(normal);
return (normal);
}
void fill_camera(t_env *env, char **tab)
{
char **split;
//double norme;
split = ft_strsplit(tab[1], ',');
env->cam_pos.x = ft_atoi(split[0] + 1);
env->cam_pos.y = ft_atoi(split[1]);
env->cam_pos.z = ft_atoi(split[2]);
ft_strsplit_clr(split);
split = ft_strsplit(tab[2], ',');
env->cam_ori.x = ft_atoi(split[0] + 1);
env->cam_ori.y = ft_atoi(split[1]);
env->cam_ori.z = ft_atoi(split[2]);
env->cam_ori = norm(env->cam_ori);
env->cam_right = vector_cross(env->cam_ori, (t_point){0, 0, 1});
env->cam_up = vector_cross(env->cam_right, env->cam_ori);
ft_strsplit_clr(split);
}
// Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p.
int get_heading(const vector *a, const vector *m, const vector *p)
{
vector E;
vector N;
// cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "east"
vector_cross(m, a, &E);
vector_normalize(&E);
// cross "down" with "east" to produce "north" (parallel to the ground)
vector_cross(a, &E, &N);
vector_normalize(&N);
// compute heading
int heading = round(atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180 / M_PI);
if (heading < 0)
heading += 360;
return heading;
}
/* class Helix3D : public Object3D */
Helix3D::Helix3D(const Color4f& cl, const Vector3D& c, const Vector3D& n, float r )
: Object3D(cl), cterm(c), nterm(n), radius(r)
{
pObj = gluNewQuadric();
gluQuadricNormals(pObj, GLU_SMOOTH);
Vector3D vec(nterm - cterm);
length = vec.length();
rotAngle = Rad2Deg* acos( vec.z / length );
Vector3D zAxis(0.0, 0.0, 1.0);
rotAxis = vector_cross(zAxis, vec);
}
/* class Strand3D : public Object3D */
Strand3D::Strand3D( const Color4f& cl, const Vector3D& c, const Vector3D& n, const Vector3D& up )
: Object3D(cl), cterm(c), nterm(n), upVector(up)
{
Vector3D vec(nterm - cterm);
length = vec.length();
rotAngle = Rad2Deg* acos( vec.z / length );
Vector3D zAxis(0.0, 0.0, 1.0);
rotAxis = vector_cross(zAxis, vec);
}
quaternion ahrs_orientation_from_accel_mag(dataexchange_t *data) {
//vectors for rotation matrix
vector3d acc_v, mag_v;
matrix3x3d rmat;
quaternion q;
acc_v.x = (*data).acc_x;
acc_v.y = (*data).acc_y;
acc_v.z = (*data).acc_z;
mag_v.x = (*data).mag_x;
mag_v.y = (*data).mag_y;
mag_v.z = (*data).mag_z;
hmc5883l_applyCalibration(&mag_v, calib_ptr);
vector3d down = vector_inv(acc_v);
vector3d east = vector_cross(down, mag_v);
vector3d north = vector_cross(east, down);
//normalize vectors
vector_norm(&down);
vector_norm(&east);
vector_norm(&north);
//vectors to matrix
matrix_vector_to_row(&rmat, north, 1);
matrix_vector_to_row(&rmat, east, 2);
matrix_vector_to_row(&rmat, down, 3);
//matrix to quaternion
q = quaternion_from_matrix(&rmat);
//normalize
quaternion_norm(&q);
(*data).qr = q;
return q;
}
/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
template <typename T> float LSM303::heading(vector<T> from)
{
vector<int32_t> temp_m = {m.x, m.y, m.z};
// subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
// compute E and N
vector<float> E;
vector<float> N;
vector_cross(&temp_m, &a, &E);
vector_normalize(&E);
vector_cross(&a, &E, &N);
vector_normalize(&N);
// compute heading
float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
return heading;
}
int RMG146::GetHeading(Vector3D from,Vector3D accel,Vector3D magnet)
{
// shift and scale
magnet.x = (magnet.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
magnet.y = (magnet.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
magnet.z = (magnet.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
// normalize
vector_normalize(&accel);
// compute E and N
Vector3D E;
Vector3D N;
vector_cross(&magnet, &accel, &E);
vector_normalize(&E);
vector_cross(&accel, &E, &N);
// compute heading
int heading = round(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI);
if (heading < 0) heading += 360;
return heading;
}
void vector_calculateNormal(vector3D_t* normalOutput, vector3D_t* dif1, vector3D_t* dif2)
{
vector3D_t t0, t1;
vector_copy(&t0, dif1);
vector_copy(&t1, dif2);
vector_normalize(&t0);
vector_normalize(&t1);
vector_cross(normalOutput, &t0, &t1);
vector_normalize(normalOutput);
}
/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
double iCompass::iheading(int ix, int iy, int iz, double ax, double ay, double az, double mx, double my, double mz)
{
if (samples == maxSamples)
{
samples = 0;
myRA.clear();
}
vector<int> from = {ix, iy, iz};
vector<double> m = {mx, my, mz};
vector<double> a = {ax, ay, az};
// compute E and N
vector<double> E;
vector<double> N;
vector_cross(&m, &a, &E);
vector_normalize(&E);
vector_cross(&a, &E, &N);
vector_normalize(&N);
// compute heading
double heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
if(heading < -9990) {
heading = 0;
}
heading = clamp360(iround(heading,1)+declinationAngle);
myRA.addValue(heading);
myRA.addValue(HeadingAvgCorr(heading, oldHeading));
oldHeading = heading;
samples++;
return myRA.getAverage();
}
// Returns the number of degrees from the From vector projected into
// the horizontal plane is away from north.
//
// Description of heading algorithm:
// Shift and scale the magnetic reading based on calibration data to
// to find the North vector. Use the acceleration readings to
// determine the Down vector. The cross product of North and Down
// vectors is East. The vectors East and North form a basis for the
// horizontal plane. The From vector is projected into the horizontal
// plane and the angle between the projected vector and north is
// returned.
int LSM303::heading(vector from)
{
// shift and scale
m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
vector temp_a = a;
// normalize
vector_normalize(&temp_a);
//vector_normalize(&m);
// compute E and N
vector E;
vector N;
vector_cross(&m, &temp_a, &E);
vector_normalize(&E);
vector_cross(&temp_a, &E, &N);
// compute heading
int heading = round(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI);
if (heading < 0) heading += 360;
return heading;
}
void glDrawCylinder(GLUquadric* pObj, const Vector3D& p1, const Vector3D& p2, double thickness, float renderDetailLevel)
{
int numSlices = (8 * renderDetailLevel);
int numStacks = 1;
static Vector3D zAxis(0.0, 0.0, 1.0);
Vector3D vec(p2 - p1);
float length = vec.length();
float rotAngle = Rad2Deg* acos( vec.z / length );
Vector3D rotAxis = vector_cross(zAxis, vec);
glPushMatrix();
glTranslatef(p1.x, p1.y, p1.z);
glRotatef(rotAngle, rotAxis.x, rotAxis.y, rotAxis.z);
gluCylinder(pObj, thickness, thickness, length, numSlices, numStacks);
glPopMatrix();
}
void Space_Object::draw_orbit() {
glPushMatrix();
float t_x;
float t_y;
this->object_orbit.focus_translate(t_x, t_y);;
glColor3f(0, 1, 0);
glBegin(GL_LINE_LOOP);
int i;
for (i = 0; i<360; i++)
{
float rad = deg_to_rad(i);
// Calculate rotation such that point will lie on new plane defined by orbital normal and the point of the parent.
GLfloat trans_norm[3];
GLfloat trans_non_normalizes[3];
trans_non_normalizes[0] = trans_norm[0] =cos(rad)*this->object_orbit.ellipse_a + t_x;
trans_non_normalizes[1] = trans_norm[1] = sin(rad)*this->object_orbit.ellipse_b + t_y;
trans_non_normalizes[2] = trans_norm[2] = 0.0f;
normalize_vector(trans_norm);
GLfloat up_norm[] = { 0, 0, 1 };
GLfloat rot_axis[3];
vector_cross(up_norm, this->orbit_plane.planeNormal, rot_axis);
GLfloat rot_angle = asin(vector_length(rot_axis));
rot_vector((rot_angle), rot_axis[0], rot_axis[1], rot_axis[2], trans_non_normalizes);
trans_non_normalizes[0] += this->parent_pos[0];
trans_non_normalizes[1] += this->parent_pos[1];
trans_non_normalizes[2] += this->parent_pos[2];
glVertex3fv(trans_non_normalizes);
}
glEnd();
glPopMatrix();
}
matrix_t matrix_create_look_at(vector_t target)
{
vector_t x_row, y_row, z_row;
z_row.x = target.x;
z_row.y = target.y;
z_row.z = target.z;
vector_normalize(&z_row);
x_row.x = 1;
x_row.y = 0;
x_row.z = -target.x/target.z;
vector_normalize(&x_row);
vector_cross(z_row, x_row, &y_row);
vector_normalize(&y_row);
matrix_t rot = matrix_create_from_rows(x_row, y_row, z_row);
return rot;
}
void vector_calculateNormal(vector3D_t* normalOutput, vector3D_t* v1, vector3D_t* v2, vector3D_t* v3)
{
//vector3D_t v0, v1, v2;
//vector_copy(&v0, &progmeshTriangle->vertex[0]->position);
//vector_copy(&v1, &progmeshTriangle->vertex[1]->position);
//vector_copy(&v2, &progmeshTriangle->vertex[2]->position);
vector3D_t t0, t1;
t0.x = v2->x - v1->x;
t0.y = v2->y - v1->y;
t0.z = v2->z - v1->z;
t1.x = v3->x - v2->x;
t1.y = v3->y - v2->y;
t1.z = v3->z - v2->z;
//vector_normalize(&t0);
//vector_normalize(&t1);
vector_cross(normalOutput, &t0, &t1);
vector_normalize(normalOutput);
}
