/*
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;
}
// 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;
}
/* Presumes that the first alm entry is the reference sat. */
s8 assign_de_mtx(u8 num_sats, const sdiff_t *sats_with_ref_first,
const double ref_ecef[3], double *DE)
{
assert(sats_with_ref_first != NULL);
assert(ref_ecef != NULL);
assert(DE != NULL);
if (num_sats <= 1) {
log_debug("assign_de_mtx: not enough sats");
return -1;
}
assert(num_sats > 1);
/* Vector to reference satellite. */
double e_0[3];
vector_subtract(3, sats_with_ref_first[0].sat_pos, ref_ecef, e_0);
vector_normalize(3, e_0);
for (u8 i=1; i<num_sats; i++) {
/* Vector to satellite i */
double e_i[3];
vector_subtract(3, sats_with_ref_first[i].sat_pos, ref_ecef, e_i);
vector_normalize(3, e_i);
/* DE row = e_i - e_0 */
vector_subtract(3, e_i, e_0, &DE[3*(i-1)]);
}
return 0;
}
// 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;
}
//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));
}
void liquid_wave_get_normal_z(map_liquid_type *liq,int div,float *wave_y,int top_add,d3vct *normal)
{
int top,top_prev,top_next,
y,y_prev,y_next;
d3vct p10,p20,n1,n2;
top=liq->top+(top_add*div);
y=(int)wave_y[div&0x3];
top_prev=top-top_add;
y_prev=(int)wave_y[(div-1)&0x3];
top_next=top+top_add;
y_next=(int)wave_y[(div+1)&0x3];
// get previous and next
// polygon normals
vector_create(&p10,liq->rgt,y_prev,top_prev,liq->lft,y_prev,top_prev);
vector_create(&p20,liq->lft,y,top,liq->lft,y_prev,top_prev);
vector_cross_product(&n1,&p10,&p20);
vector_normalize(&n1);
vector_create(&p10,liq->rgt,y,top,liq->lft,y,top);
vector_create(&p20,liq->lft,y_next,top_next,liq->lft,y,top);
vector_cross_product(&n2,&p10,&p20);
vector_normalize(&n2);
// average for normal
normal->x=(n1.x+n2.x)*0.5f;
normal->y=(n1.y+n2.y)*0.5f;
normal->z=(n1.z+n2.z)*0.5f;
vector_normalize(normal);
}
// 设置摄像机
void matrix_set_lookat(matrix_t *m, const vector_t *eye, const vector_t *at, const vector_t *up) {
vector_t xaxis, yaxis, zaxis;
vector_sub(&zaxis, at, eye);
vector_normalize(&zaxis);
vector_crossproduct(&xaxis, up, &zaxis);
vector_normalize(&xaxis);
vector_crossproduct(&yaxis, &zaxis, &xaxis);
m->m[0][0] = xaxis.x;
m->m[1][0] = xaxis.y;
m->m[2][0] = xaxis.z;
m->m[3][0] = -vector_dotproduct(&xaxis, eye);
m->m[0][1] = yaxis.x;
m->m[1][1] = yaxis.y;
m->m[2][1] = yaxis.z;
m->m[3][1] = -vector_dotproduct(&yaxis, eye);
m->m[0][2] = zaxis.x;
m->m[1][2] = zaxis.y;
m->m[2][2] = zaxis.z;
m->m[3][2] = -vector_dotproduct(&zaxis, eye);
m->m[0][3] = m->m[1][3] = m->m[2][3] = 0.0f;
m->m[3][3] = 1.0f;
}
GEOM geom_3v_onto_3v(VECTOR v1, VECTOR v2, VECTOR v3, VECTOR t1, VECTOR t2, VECTOR t3)
{ GEOM op;
float angle;
LINE axis;
VECTOR v123;
PLANE plane_v123, plane_t123;
/* step 1, superimpose v1 to t1 */
geom_reset(&op);
geom_move(&op, vector_vminusv(t1, v1));
/* step 2, align v1-v2 to t1-t2 */
angle = all(line_2v(v1, v2), line_2v(t1, t2));
axis.p0 = t1;
axis.t = vector_normalize(vector_vxv(vector_vminusv(v2, v1), vector_vminusv(t2, t1)));
geom_roll(&op, angle, axis);
/* normal direction of plane v123 should be updated */
geom_apply(op, &v1);
geom_apply(op, &v2);
geom_apply(op, &v3);
plane_v123 = plane_3v(v1, v2, v3);
plane_t123 = plane_3v(t1, t2, t3);
v123 = plane_v123.t;
/*step 3, align v1-v2-v3 to t1-t2-t3 */
angle = avv(v123, plane_t123.t);
axis.p0 = t1;
axis.t = vector_normalize(vector_vxv(v123, plane_t123.t));
geom_roll(&op, angle, axis);
return op;
}
/*
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;
}
static INLINE double vector_angles(double *v0,
double *v1)
{
double dot = vector_dot(v0, v1);
double norm0 = vector_normalize(v0);
double norm1 = vector_normalize(v1);
return acos(dot / (norm0 * norm1));
}
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);
}
}
Bool find_folding(float A[3],float B[3],float C[3],
float D[3],float E[3],float F[3],
float V[3])
/* set V so that AV is the line, where AD meets AE while
AD and AE are rotarted around AB and AC respectively,
and F and V are on the same side of the plane ABC.
*/
{
float A1[3], A2[3], B1[3], B2[3], K[3];
if(vector_eq(A,B))
{
printf("FOLDING REFUSED: A=B !!!\n");
return False;
}
vector_sub(B,A, A1);
vector_normalize(A1);
if(vector_eq(A,C))
{
printf("FOLDING REFUSED: A=C !!!\n");
return False;
}
vector_sub(C,A, A2);
vector_normalize(A2);
if(vector_eq(A,D))
{
printf("FOLDING REFUSED: A=D !!!\n");
return False;
}
vector_sub(D,A, B1);
vector_normalize(B1);
if(vector_eq(A,E))
{
printf("FOLDING REFUSED: A=E !!!\n");
return False;
}
vector_sub(E,A, B2);
vector_normalize(B2);
vector_sub(F,A, K);
if(find_centered_folding(A1,A2, B1,B2, K,V))
{
vector_add(A,V, V);
return True;
}
else return False;
}
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);
}
void Bullet::step(float t, float speed, const sf::Vector2f& target)
{
pos += vel * t;
switch (type) {
case ACCELERATING:
vel *= (float)pow(ACCEL_RATE, t);
break;
case TRACKING:
vel += vector_normalize(target - pos) * (float)(TRACK_PARAM * t);
vel = vector_normalize(vel) * speed;
break;
}
}
static u8 get_de_and_phase(sats_management_t *sats_man,
u8 num_sdiffs, sdiff_t *sdiffs,
double ref_ecef[3],
double *de, double *phase)
{
u8 ref_prn = sats_man->prns[0];
u8 num_sats = sats_man->num_sats;
double e0[3];
double phi0 = 0;
/* TODO: Detect if ref_prn is not in prns and return error? */
u8 i;
for (i=0; i<num_sdiffs; i++) {
if (sdiffs[i].prn == ref_prn) {
e0[0] = sdiffs[i].sat_pos[0] - ref_ecef[0];
e0[1] = sdiffs[i].sat_pos[1] - ref_ecef[1];
e0[2] = sdiffs[i].sat_pos[2] - ref_ecef[2];
vector_normalize(3, e0);
phi0 = sdiffs[i].carrier_phase;
break;
}
}
i=1;
u8 j = 0;
while (i < num_sats) {
if (sdiffs[j].prn < sats_man->prns[i]) {
j++;
}
else if (sdiffs[j].prn > sats_man->prns[i]) {
/* This should never happen. */
log_warn("sdiffs should be a super set of sats_man prns\n");
i++;
}
else { /* else they match */
double e[3];
e[0] = sdiffs[j].sat_pos[0] - ref_ecef[0];
e[1] = sdiffs[j].sat_pos[1] - ref_ecef[1];
e[2] = sdiffs[j].sat_pos[2] - ref_ecef[2];
vector_normalize(3, e);
de[(i-1)*3 ] = e[0] - e0[0];
de[(i-1)*3 + 1] = e[1] - e0[1];
de[(i-1)*3 + 2] = e[2] - e0[2];
phase[i-1] = sdiffs[j].carrier_phase - phi0;
i++;
j++;
}
}
return num_sats;
}
static u8 get_de_and_phase(sats_management_t *sats_man,
u8 num_sdiffs, sdiff_t *sdiffs,
double ref_ecef[3],
double *de, double *phase)
{
gnss_signal_t ref_sid = sats_man->sids[0];
u8 num_sats = sats_man->num_sats;
double e0[3];
double phi0 = 0;
/* TODO: Detect if ref_sid is not in sids and return error? */
u8 i;
for (i=0; i<num_sdiffs; i++) {
if (sid_is_equal(sdiffs[i].sid, ref_sid)) {
e0[0] = sdiffs[i].sat_pos[0] - ref_ecef[0];
e0[1] = sdiffs[i].sat_pos[1] - ref_ecef[1];
e0[2] = sdiffs[i].sat_pos[2] - ref_ecef[2];
vector_normalize(3, e0);
phi0 = sdiffs[i].carrier_phase;
break;
}
}
i=1;
u8 j = 0;
while (i < num_sats) {
if (sid_compare(sdiffs[j].sid, sats_man->sids[i]) < 0) {
j++;
}
else if (sid_compare(sdiffs[j].sid, sats_man->sids[i]) > 0) {
/* This should never happen. */
log_warn("sdiffs should be a super set of sats_man sids");
i++;
}
else { /* else they match */
double e[3];
e[0] = sdiffs[j].sat_pos[0] - ref_ecef[0];
e[1] = sdiffs[j].sat_pos[1] - ref_ecef[1];
e[2] = sdiffs[j].sat_pos[2] - ref_ecef[2];
vector_normalize(3, e);
de[(i-1)*3 ] = e[0] - e0[0];
de[(i-1)*3 + 1] = e[1] - e0[1];
de[(i-1)*3 + 2] = e[2] - e0[2];
phase[i-1] = sdiffs[j].carrier_phase - phi0;
i++;
j++;
}
}
return num_sats;
}
Matrix* matrix_qr_decomposition(Matrix M)
{
assert(is_matrix(M));
int m = M->r;
int n = M->c;
Matrix Q = matrix_new_empty(m, n);
for(int i = 0; i < n; i++) {
double* u = matrix_column_vector(M, i);
double* a = matrix_column_vector(M, i);
for(int k = 0; k < i; k++) {
double* e = matrix_column_vector(Q, k);
double* p = vector_projection(e, a, m);
free(e);
for(int j = 0; j < m; j++)
u[j] -= p[j];
free(p);
}
free(a);
double* e = vector_normalize(u, m);
free(u);
for(int l = 0; l < m; l++)
Q->A[l][i] = e[l];
free(e);
}
Matrix Qt = matrix_transpose(Q);
Matrix R = matrix_multiply(Qt, M);
matrix_free(Qt);
Matrix* QR = calloc(2, sizeof(Matrix));
QR[0] = Q;
QR[1] = R;
return QR;
}
void simulation_render_lagrange_points(simulation_t* sim,celestial_body_t* secondary)
{
vector_t lagrange_points[5];
vector_t rel_position=vector_subtract(secondary->base.position,secondary->orbit.primary->base.position);
vector_t direction=vector_normalize(rel_position);
float a=secondary->base.mass/(secondary->base.mass+secondary->orbit.primary->base.mass);
float offset=powf(a*0.3333333,0.333333);
lagrange_points[0]=vector_multiply(secondary->orbit.semi_major_axis*(1+offset),direction);
lagrange_points[1]=vector_multiply(secondary->orbit.semi_major_axis*(1-offset),direction);
lagrange_points[2]=vector_multiply(-secondary->orbit.semi_major_axis*(1+(5.0/12.0)*a),direction);
lagrange_points[3].x=rel_position.x*cos(M_PI/3)+rel_position.y*sin(M_PI/3);
lagrange_points[3].y=rel_position.x*-sin(M_PI/3)+rel_position.y*cos(M_PI/3);
lagrange_points[4].x=rel_position.x*cos(-M_PI/3)+rel_position.y*sin(-M_PI/3);
lagrange_points[4].y=rel_position.x*-sin(-M_PI/3)+rel_position.y*cos(-M_PI/3);
int i;
for(i=0;i<5;i++)
{
char str[128];
sprintf(str,"%s-%s L%d point",secondary->orbit.primary->name,secondary->name,i+1);
vector_t screen_pos=vector_transform(vector_add(secondary->orbit.primary->base.position,lagrange_points[i]),sim->camera);
draw_cross(screen_pos,get_color(0,0,255));
draw_text(screen_pos.x+10,screen_pos.y,str);
}
}
void make_random_unit_quaternion(gsl_vector *Q, gsl_rng *rng)
{
/* This comes from Graphics Gems III p. 129. */
for (size_t i = 0; i < 4; i++)
gsl_vector_set(Q, i, gsl_ran_ugaussian(rng));
vector_normalize(Q);
}
void plan_new(t_vector *normal, int constante, int color)
{
t_data *data;
t_plan *plan;
t_plan *tmp;
data = data_init();
if ((plan = (t_plan *)ft_memalloc(sizeof(t_plan))) == NULL)
ft_error("error malloc");
plan->type = 'p';
plan->color = color;
plan->ref = (color >> 24) % 256;
plan->normal = normal;
vector_normalize(plan->normal);
plan->constante = constante;
plan->next = NULL;
if (data->plan)
{
tmp = data->plan;
while (tmp->next != NULL)
tmp = tmp->next;
tmp->next = plan;
}
else
data->plan = plan;
}
int hit_cylinder(t_ray *r, t_cylinder cy, double *hit)
{
t_lanormemecasselescouilles q;
double tmp;
t_vector new_o;
double sucemanorme[3];
if (cy.dir.x == 0 && cy.dir.y == 0 && cy.dir.z == 0)
return (0);
cy.dir = vector_normalize(cy.dir);
new_o = vector_sub(r->o, cy.o);
sucemanorme[0] = vector_dot_product(r->dir, cy.dir);
sucemanorme[1] = vector_dot_product(new_o, cy.dir);
sucemanorme[2] = cy.rayon;
if (get_eqcyl(r, sucemanorme, new_o, &q) && q.eqc[3] < 0)
return (0);
else if ((tmp = (-q.eqc[1] - sqrt(q.eqc[3])) / (2 * q.eqc[0])) || 1)
{
if (tmp < 0 &&
((tmp = (-q.eqc[1] + sqrt(q.eqc[3])) / (2 * q.eqc[0])) || 1))
if (tmp < 0)
return (0);
if (*hit > 0 && tmp > *hit)
return (0);
*hit = tmp;
return (1);
}
}
int hit_cone(t_ray *r, t_cone co, double *hit)
{
t_lanormemecasselescouilles q;
double tmp;
t_vector new_o;
double norm_sucks[3];
co.dir = vector_normalize(co.dir);
new_o = vector_sub(r->o, co.o);
norm_sucks[2] = 1 + tan(co.angle) * tan(co.angle);
norm_sucks[0] = vector_dot_product(r->dir, co.dir);
norm_sucks[1] = vector_dot_product(new_o, co.dir);
get_eqcone(r, norm_sucks, new_o, &q);
if (q.eqc[3] < 0)
return (0);
else if ((tmp = (-q.eqc[1] - sqrt(q.eqc[3])) / (2 * q.eqc[0])) || 1)
{
if (tmp < 0 &&
((tmp = (-q.eqc[1] + sqrt(q.eqc[3]) / (2 * q.eqc[0]))) || 1))
if (tmp < 0)
return (0);
if (*hit > 0 && tmp > *hit)
return (0);
*hit = tmp;
return (1);
}
}
Vector triangle_normal(Vector a, Vector b, Vector c)
{
Vector ab = SUB(b, a);
Vector ac = SUB(c, a);
return vector_normalize(cross(ab, ac));
}
void ViewBase::update_view() { //biggest cube edge is 2 and center == [0,0,0]
view.origin = vector_rotate(cam_pos, cam_matrix);
view.direction = vector_normalize(-view.origin);
float step_px = virtual_view_size / MINIMUM(view.dims.x, view.dims.y);
view.right_plane = vector_rotate(make_float4(1, 0, 0, 0), cam_matrix) * step_px;
view.up_plane = vector_rotate(make_float4(0, 1, 0, 0), cam_matrix) * step_px;
}
static INLINE double vector_orientation(double *v)
{
double norm = vector_normalize(v);
double cosa = v[0] / norm;
double sina = v[1] / norm;
return (sina>=0 ? acos(cosa) : 2*M_PI - acos(cosa));
}
extern void matrix_solve(vector* v, const matrix* m)
{
assert(v != NULL);
assert(m != NULL);
assert(v->size == m->size);
size_t size = m->size;
vector tmp_vec;
vector_init(&tmp_vec, size);
for (size_t i = 0; i < size; ++i)
{
v->elements[i] = 1.0;
tmp_vec.elements[i] = 0.0;
}
for(size_t x = 0; x < 3; ++x)
{
matrix_multiply(&tmp_vec, m, v);
vector_normalize(&tmp_vec);
for (size_t i = 0; i < size; ++i)
{
v->elements[i] = tmp_vec.elements[i];
tmp_vec.elements[i] = 0.0;
}
}
vector_free(&tmp_vec);
}
Bool find_three_point_transform(float P1[3], float P2[3], float P3[3],
float Q1[3], float Q2[3], float Q3[3],
float mv[3], float R[3][3])
/* assumption: for each Pi!=Pj and Qi!=Qj */
{
float u[3][3], w[3][3];
int i,j,k;
vector_sub(P2,P1, u[0]);
vector_sub(P3,P1, u[1]);
vector_product(u[0], u[1], u[1]); /* u[1] = (P2-P1)x(P3-P1) */
vector_normalize(u[0]);
vector_normalize(u[1]);
vector_product(u[0], u[1], u[2]); /* u[2] |_ u[1] |_ u[2] */
if(vector_length(u[2])==0)
{
printf("three points transformation: the first three points are colinear !!!\n");
return False;
}
vector_sub(Q2,Q1, w[0]);
vector_sub(Q3,Q1, w[1]);
vector_product(w[0], w[1], w[1]); /* w[1] = (Q2-Q1)x(Q3-Q1) */
vector_normalize(w[0]);
vector_normalize(w[1]);
vector_product(w[0], w[1], w[2]); /* w[2] |_ w[1] |_ w[2] */
if(vector_length(w[2])==0)
{
printf("three points transformation: the last three points are colinear !!!\n");
return False;
}
for(i=0; i<3; i++)
for(j=0; j<3; j++)
{
R[i][j]=0;
for(k=0; k<3; k++)
R[i][j]+=u[k][j]*w[k][i];
}
matrix3_vector_mult(R, P1, mv);
vector_sub(Q1, mv, mv); /* mv = Q1 - R*P1 */
return True;
}
void vector_create(d3vct *v,int x1,int y1,int z1,int x2,int y2,int z2)
{
v->x=(float)(x1-x2);
v->y=(float)(y1-y2);
v->z=(float)(z1-z2);
vector_normalize(v);
}
void vector_cross_product(d3vct *v,d3vct *v1,d3vct *v2)
{
v->x=(v1->y*v2->z)-(v2->y*v1->z);
v->y=(v1->z*v2->x)-(v2->z*v1->x);
v->z=(v1->x*v2->y)-(v2->x*v1->y);
vector_normalize(v);
}
void matrix_lookat( matrix m, vector position, vector target, float roll ){
vector up, forward, right;
matrix temp;
up = vector_make( (float)sin(roll), (float)-cos(roll), 0 );
forward = vector_normalize(vector_sub(target,position));
right = vector_normalize(vector_crossproduct(up,forward));
up = vector_normalize(vector_crossproduct(right,forward));
m[0 ] = right.x; m[1 ] = up.x; m[2 ] = forward.x; m[3 ] = 0;
m[4 ] = right.y; m[5 ] = up.y; m[6 ] = forward.y; m[7 ] = 0;
m[8 ] = right.z; m[9 ] = up.z; m[10] = forward.z; m[11] = 0;
m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1;
matrix_translate( temp, vector_scale(position, -1.f) );
matrix_multiply( m, temp );
}