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);
}
}
void vector_difference_to_euler_angles(fixedpointnum *v1, fixedpointnum *v2, fixedpointnum *euler) {
// take the difference between the two attitudes and return the euler angles between them
// find the axis of rotation and angle between the two downVectors
// the cross products of the two vectors will give us the axis of rotation from one to the other
fixedpointnum axisofrotation[3];
vector_cross_product(v1, v2, axisofrotation);
fixedpointnum axislength=lib_fp_sqrt(normalize_vector(axisofrotation));
// get the angle of rotation between the two vectors
fixedpointnum angle=lib_fp_atan2(axislength, vector_dot_product(v1, v2));
fixedpointnum unitvector[3];
unitvector[0]=0;
unitvector[1]=FIXEDPOINTONE;
unitvector[2]=0;
euler[0]=lib_fp_multiply(vector_dot_product(axisofrotation, unitvector), angle);
unitvector[0]=FIXEDPOINTONE;
unitvector[1]=0;
unitvector[2]=0;
euler[1]=lib_fp_multiply(vector_dot_product(axisofrotation, unitvector), angle);
}
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 get_eqcone(t_ray *r, double n[3], t_vector new_o,
t_lanormemecasselescouilles *q)
{
q->eqc[0] = vector_dot_product(r->dir, r->dir) - n[2] * n[0] * n[0];
q->eqc[1] = 2 * (vector_dot_product(r->dir, new_o) - n[2] * n[0] * n[1]);
q->eqc[2] = vector_dot_product(new_o, new_o) - n[2] * n[1] * n[1];
q->eqc[3] = q->eqc[1] * q->eqc[1] - 4 * q->eqc[0] * q->eqc[2];
return (1);
}
void cylinder_inter(t_inter *inter, void *obj, t_ray ray,
t_light *light)
{
t_cylinder cylinder;
double dist;
t_ray temp;
cylinder = *((t_cylinder *)obj);
temp = ray;
change_frame(&ray, cylinder.inv, vector_inverse(cylinder.trans));
dist = cylinder_distance(ray, cylinder);
if (dist > EPS && (inter->dist == NULL || dist < *(inter->dist) - EPS))
{
if (inter->dist == NULL)
inter->dist = malloc(sizeof(double));
free_light_ray_list(inter);
*(inter->dist) = dist;
inter->pos = calculate_position(ray, dist);
inter->normal = cylinder_normal(inter->pos, cylinder);
if (vector_dot_product(inter->normal, ray.dir) > 0)
inter->normal = vector_inverse(inter->normal);
op_inv(cylinder.trans, cylinder.rot, &(inter->normal), &(inter->pos));
inter->refl = calculate_reflection(temp, inter->normal);
inter->color = cylinder.color;
create_light_ray_list(inter, light, inter->pos);
inter->refl_val = cylinder.refl;
}
}
int hit_sphere(t_ray *r, t_sphere s, double *hit)
{
double eqc[4];
double tmp;
eqc[0] = vector_dot_product(r->dir, r->dir);
eqc[1] = 2 * (r->dir.x * (r->o.x - s.o.x) + r->dir.y * (r->o.y - s.o.y) +
r->dir.z * (r->o.z - s.o.z));
eqc[2] = (r->o.x - s.o.x) * (r->o.x - s.o.x) + (r->o.y - s.o.y) *
(r->o.y - s.o.y) + (r->o.z - s.o.z) * (r->o.z - s.o.z) -
s.rayon * s.rayon;
eqc[3] = eqc[1] * eqc[1] - 4 * eqc[0] * eqc[2];
if (eqc[3] < 0)
return (0);
else
{
tmp = (-eqc[1] - sqrt(eqc[3])) / (2 * eqc[0]);
if (tmp < 0 && ((tmp += sqrt(eqc[3]) / eqc[0]) || 1))
if (tmp < 0)
return (0);
if (*hit > 0 && tmp > *hit)
return (0);
*hit = tmp;
return (1);
}
}
/*
% project v in direction of u
function p=project_vec(v,u)
p = (dot(v,u)/norm(u)^2)*u;
*/
void project_vector(vec *v, vec *u, vec *p){
double dot_product_val, vec_norm, scalar_val;
dot_product_val = vector_dot_product(v, u);
vec_norm = vector_get2norm(u);
scalar_val = dot_product_val/(vec_norm*vec_norm);
vector_copy(p, u);
vector_scale(p, scalar_val);
}
void object_render_flatshading(surface_t* s, object_t* obj, vector_t* pbuffer, char* stencil, vector_t* light) {
int c, diff;
//int l, top, bottom;
vector_t *i, *j, *k;
vector_t q1, q2, N;
mesh_t* mesh = obj->mesh;
vector_t* m = mesh->points;
triangle_t* t = mesh->triangles;
int li;
object_apply_transformations(obj, pbuffer, 128, 96); // FIXME: must use surface center
m = mesh->points;
diff = (char*)pbuffer - (char*)m;
for (c = 0; c < mesh->tcount; c++) {
i = (vector_t*)((char*)t->vertexes[0] + diff);
j = (vector_t*)((char*)t->vertexes[1] + diff);
k = (vector_t*)((char*)t->vertexes[2] + diff);
vector_subtract(j, i, &q1);
vector_subtract(k, i, &q2);
vector_cross_product(&q1, &q2, &N);
if (N.z > 0) {
vector_normalize(&N, &N);
//li = vector_dot_product(&N, light)*5;
//li = f2i(li);
//li = (li < 0) ? 0 : ((li > 4) ? 4 : li);
// FIXME: can optimize memset's by covering only "top" to "bottom" lines
// in this case we should require clean buffers to start with.
// and we should clean them after rendering.
//memset(low, MODE2_HEIGHT << 1, 64); // FIXME: must use surface height
//memset(high, MODE2_HEIGHT << 1, 0); // FIXME: must use surface height
stencil_init(stencil);
// calculate polygon
stencil_add_side(i->x, i->y, j->x, j->y, stencil);
stencil_add_side(j->x, j->y, k->x, k->y, stencil);
stencil_add_side(k->x, k->y, i->x, i->y, stencil);
/*
top = (i->y < j->y) ? ((i->y < k->y) ? i->y : k->y) : ((j->y < k->y) ? j->y : k->y);
bottom = (i->y > j->y) ? ((i->y > k->y) ? i->y : k->y) : ((j->y > k->y) ? j->y : k->y);
for (l = top; l <= bottom; l++) {
surface_hline(s, low[l], l, high[l], DITHER(li, l));
}
*/
surface_stencil_render(s, stencil, vector_dot_product(&N, light)/6);
//surface_stencil_render(s, stencil, li);
}
t++;
}
}
/*******************************************************************************
* vector_t vector_projection(vector_t v, vector_t e)
*
* Projects vector v onto e
*******************************************************************************/
vector_t vector_projection(vector_t v, vector_t e){
int i;
float factor;
vector_t out = create_empty_vector();
if(!v.initialized || !e.initialized){
printf("ERROR: vectors not initialized yet\n");
return out;
}
if(v.len != e.len){
printf("ERROR: vectors not of same dimension\n");
return out;
}
out = create_vector(v.len);
factor = vector_dot_product(v,e)/vector_dot_product(e,e);
for(i=0;i<v.len;i++){
out.data[i] = factor * e.data[i];
}
return out;
}
int hit_plane(t_ray *r, t_plane p, double *hit)
{
double a;
double b;
p.n = vector_normalize(p.n);
a = vector_dot_product(r->dir, p.n);
b = vector_dot_product(p.n, vector_sub(r->o, p.pt));
if (a == 0)
return (0);
if (*hit > 0 && -b / a > *hit)
return (0);
else if (-b / a > 0)
{
*hit = -b / a;
return (1);
}
else
return (0);
}
void rotate_vector_by_axis_angle(fixedpointnum *v1, fixedpointnum *axisvector, fixedpointnum angle, fixedpointnum *v2) {
fixedpointnum cosineofangle=lib_fp_cosine(angle);
fixedpointnum sineofangle=lib_fp_sine(angle);
fixedpointnum crossproductvector[3];
vector_cross_product(axisvector,v1, crossproductvector);
fixedpointnum dotproducttimesoneminuscosineofangle=lib_fp_multiply(vector_dot_product(axisvector,v1),FIXEDPOINTONE-cosineofangle);
v2[0]=lib_fp_multiply(v1[0], cosineofangle)+lib_fp_multiply(crossproductvector[0],sineofangle)+lib_fp_multiply(axisvector[0],dotproducttimesoneminuscosineofangle);
v2[1]=lib_fp_multiply(v1[1], cosineofangle)+lib_fp_multiply(crossproductvector[1],sineofangle)+lib_fp_multiply(axisvector[1],dotproducttimesoneminuscosineofangle);
v2[2]=lib_fp_multiply(v1[2], cosineofangle)+lib_fp_multiply(crossproductvector[2],sineofangle)+lib_fp_multiply(axisvector[2],dotproducttimesoneminuscosineofangle);
}
static void matrix_mult_vector(matrix_t * A, vector_t * x, vector_t * b)
{
/* Ax = b */
int i;
vector_t * tmp_v = vector_create_with_data(x->size, NULL);
for (i = 0; i < A->rows; i++)
{
/* treat A's row as a vector */
tmp_v->data = A->data[i];
/* run a dot product and store in b */
b->data[i] = vector_dot_product(tmp_v, x);
}
tmp_v->data = NULL;
vector_destroy(tmp_v);
}
/*******************************************************************************
* matrix_t householder_matrix(vector_t v)
*
* returns the householder reflection matrix for a given vector
*******************************************************************************/
matrix_t householder_matrix(vector_t v){
int i, j;
float tau;
matrix_t out = create_empty_matrix();
if(!v.initialized){
printf("ERROR: vector not initialized yet\n");
return out;
}
out = create_square_matrix(v.len);
for(i=0;i<v.len;i++){
out.data[i][i] = 1;
}
tau = 2.0/vector_dot_product(v,v);
for(i=0;i<v.len;i++){
for(j=0;j<v.len;j++){
out.data[i][j] -= tau * v.data[i]*v.data[j];
}
}
return out;
}
void map_prepare_mesh_poly(map_type *map,map_mesh_type *mesh,map_mesh_poly_type *poly)
{
int n,ptsz,y,lx,rx,lz,rz,dist;
float ang;
bool flat;
d3pnt min,max,mid;
d3pnt *pt;
d3vct map_up;
// find enclosing square
// and middle and if polygon is flat
pt=&mesh->vertexes[poly->v[0]];
min.x=max.x=mid.x=pt->x;
min.y=max.y=mid.y=y=pt->y;
min.z=max.z=mid.z=pt->z;
flat=TRUE;
ptsz=poly->ptsz;
for (n=1;n<ptsz;n++) {
pt=&mesh->vertexes[poly->v[n]];
// get min and max
if (pt->x<min.x) min.x=pt->x;
if (pt->x>max.x) max.x=pt->x;
if (pt->y<min.y) min.y=pt->y;
if (pt->y>max.y) max.y=pt->y;
if (pt->z<min.z) min.z=pt->z;
if (pt->z>max.z) max.z=pt->z;
// add for middle
mid.x+=pt->x;
mid.y+=pt->y;
mid.z+=pt->z;
// check for flat y
if (pt->y!=y) flat=FALSE;
}
memmove(&poly->box.min,&min,sizeof(d3pnt));
memmove(&poly->box.max,&max,sizeof(d3pnt));
poly->box.mid.x=mid.x/ptsz;
poly->box.mid.y=mid.y/ptsz;
poly->box.mid.z=mid.z/ptsz;
poly->box.flat=flat;
// get dot product of normal and up
// vector to determine the slope of surface
// and if it's wall like
if (flat) {
poly->slope.y=0.0f;
poly->slope.ang_y=0.0f;
poly->slope.move_x=0.0f;
poly->slope.move_z=0.0f;
poly->box.wall_like=FALSE;
}
else {
map_up.x=0.0f;
map_up.y=-1.0f;
map_up.z=0.0f;
ang=fabsf(vector_dot_product(&map_up,&poly->tangent_space.normal));
// use dot product to tell if wall like
// and the y slope
poly->box.wall_like=(ang<=0.4f);
poly->slope.y=1.0f-ang;
// find the slope angle
map_prepare_mesh_poly_slope_ang(mesh,poly);
angle_get_movement_float(poly->slope.ang_y,(map->physics.slope_max_speed*poly->slope.y),&poly->slope.move_x,&poly->slope.move_z);
}
// create wall "line" for wall like polygons
if (poly->box.wall_like) {
// get the lx,lz to rx,rz
lx=poly->box.min.x;
rx=poly->box.max.x;
lz=poly->box.min.z;
rz=poly->box.max.z;
for (n=0;n!=poly->ptsz;n++) {
pt=&mesh->vertexes[poly->v[n]];
if ((rx-lx)>(rz-lz)) {
if (pt->x==lx) lz=pt->z;
if (pt->x==rx) rz=pt->z;
}
else {
if (pt->z==lz) lx=pt->x;
if (pt->z==rz) rx=pt->x;
}
}
poly->line.lx=lx;
poly->line.rx=rx;
poly->line.lz=lz;
poly->line.rz=rz;
// find ty,by for each point
// we need to catch polygons that have higher or lower
// points slightly offset from lx,lz or rx,rz, so we use
// a distance calculation here (within 20% of end point)
dist=distance_2D_get(lx,lz,rx,rz);
dist=(dist*20)/100;
poly->line.l_ty=poly->line.r_ty=poly->line.l_by=poly->line.r_by=-1;
for (n=0;n!=poly->ptsz;n++) {
pt=&mesh->vertexes[poly->v[n]];
if (distance_2D_get(pt->x,pt->z,lx,lz)<dist) {
if ((pt->y<poly->line.l_ty) || (poly->line.l_ty==-1)) poly->line.l_ty=pt->y;
if ((pt->y>poly->line.l_by) || (poly->line.l_by==-1)) poly->line.l_by=pt->y;
}
if (distance_2D_get(pt->x,pt->z,rx,rz)<dist) {
if ((pt->y<poly->line.r_ty) || (poly->line.r_ty==-1)) poly->line.r_ty=pt->y;
if ((pt->y>poly->line.r_by) || (poly->line.r_by==-1)) poly->line.r_by=pt->y;
}
}
if (poly->line.l_ty==-1) poly->line.l_ty=poly->box.min.y;
if (poly->line.r_ty==-1) poly->line.r_ty=poly->box.min.y;
if (poly->line.l_by==-1) poly->line.l_by=poly->box.max.y;
if (poly->line.r_by==-1) poly->line.r_by=poly->box.max.y;
}
// get the plane equation for ray-plane intersections
map_prepare_mesh_poly_plane(mesh,poly);
// determine if shadows can project on this polygon
map_prepare_mesh_poly_shadow(map,mesh,poly);
}
bool model_recalc_normals_determine_vector_in_out(model_type *model,int mesh_idx,int vertex_idx)
{
int x,y,z,k,pos_dist,neg_dist;
float f_dist;
bool is_out;
d3pnt *pnt,min,max,center,pos_pt,neg_pt;
d3vct face_vct;
model_mesh_type *mesh;
model_vertex_type *vertex;
// get box for vertex. This will be the combination
// of vertexes with the same material
map_recalc_normals_get_vertex_box(model,mesh_idx,vertex_idx,&min,&max);
// get the box center
mesh=&model->meshes[mesh_idx];
vertex=&mesh->vertexes[vertex_idx];
pnt=&vertex->pnt;
center.x=(min.x+max.x)>>1;
center.y=(min.y+max.y)>>1;
center.z=(min.z+max.z)>>1;
// the dot product is the fall back position
// if these specialized checks fail
vector_create(&face_vct,pnt->x,pnt->y,pnt->z,center.x,center.y,center.z);
is_out=(vector_dot_product(&vertex->tangent_space.normal,&face_vct)>0.0f);
// get a point from the current normal vector
// and inverse of the current normal vector, using 10%
// of the distance to center
f_dist=(float)distance_get(pnt->x,pnt->y,pnt->z,center.x,center.y,center.z);
f_dist*=0.1f;
pos_pt.x=pnt->x+(int)(vertex->tangent_space.normal.x*f_dist);
pos_pt.y=pnt->y+(int)(vertex->tangent_space.normal.y*f_dist);
pos_pt.z=pnt->z+(int)(vertex->tangent_space.normal.z*f_dist);
neg_pt.x=pnt->x-(int)(vertex->tangent_space.normal.x*f_dist);
neg_pt.y=pnt->y-(int)(vertex->tangent_space.normal.y*f_dist);
neg_pt.z=pnt->z-(int)(vertex->tangent_space.normal.z*f_dist);
// first we determine if we can think of the
// poly's box (which is determined by all connected
// polys) as a closed object in one direction
// if one direction is at least 25% greater than the others
// then consider it a tube like structure
// if any distance calcs fail, fall back to dot product
x=max.x-min.x;
y=max.y-min.y;
z=max.z-min.z;
k=x-((x*25)/100);
if ((x>y) && (x>z)) {
pos_dist=distance_2D_get(pos_pt.y,pos_pt.z,center.y,center.z);
neg_dist=distance_2D_get(neg_pt.y,neg_pt.z,center.y,center.z);
if (pos_dist==neg_dist) return(is_out);
return(pos_dist>neg_dist);
}
k=y-((y*25)/100);
if ((y>x) && (y>z)) {
pos_dist=distance_2D_get(pos_pt.x,pos_pt.z,center.x,center.z);
neg_dist=distance_2D_get(neg_pt.x,neg_pt.z,center.x,center.z);
if (pos_dist==neg_dist) return(is_out);
return(pos_dist>neg_dist);
}
k=z-((z*25)/100);
if ((z>x) && (z>y)) {
pos_dist=distance_2D_get(pos_pt.x,pos_pt.y,center.x,center.y);
neg_dist=distance_2D_get(neg_pt.x,neg_pt.y,center.x,center.y);
if (pos_dist==neg_dist) return(is_out);
return(pos_dist>neg_dist);
}
// finally fall back to dot product
return(is_out);
}
bool object_move_xz_slide_line(obj_type *obj,int *xadd,int *yadd,int *zadd,int lx,int rx,int lz,int rz)
{
int n,xadd2,yadd2,zadd2,mx,mz;
float f,ang,rang;
bool hit,cwise;
d3vct line_vct,obj_vct;
// special check for horizontal/vertical walls
if (lx==rx) {
xadd2=0;
yadd2=0;
zadd2=*zadd;
if (collide_object_to_map(obj,&xadd2,&yadd2,&zadd2)) return(FALSE);
obj->pnt.z+=zadd2;
xadd2=*xadd;
yadd2=0;
zadd2=0;
if (collide_object_to_map(obj,&xadd2,&yadd2,&zadd2)) return(FALSE);
obj->pnt.x+=xadd2;
return(FALSE);
}
if (lz==rz) {
xadd2=*xadd;
yadd2=0;
zadd2=0;
if (collide_object_to_map(obj,&xadd2,&yadd2,&zadd2)) return(FALSE);
obj->pnt.x+=xadd2;
xadd2=0;
yadd2=0;
zadd2=*zadd;
if (collide_object_to_map(obj,&xadd2,&yadd2,&zadd2)) return(FALSE);
obj->pnt.z+=zadd2;
return(FALSE);
}
// get angle between the line and the object movement
obj_vct.x=(float)*xadd;
obj_vct.y=0.0f;
obj_vct.z=(float)*zadd;
vector_normalize(&obj_vct);
vector_create(&line_vct,lx,0,lz,rx,0,rz); // perpendicular vector (swap x/z)
// get the angle between them
f=vector_dot_product(&obj_vct,&line_vct);
f=((float)acos(f))*RAD_to_ANG;
// get angle of wall. If the angle between the
// collision lines is less than 90, then head down
// the line the opposite way
ang=angle_find(lx,lz,rx,rz);
if (f<90.0f) ang=angle_add(ang,180.0f);
// change the motion to reflect the angle of the wall
// uses facing instead of motion to check so we
// don't get motion build up from sliding against a wall
rang=obj->ang.y;
if (obj->forward_move.reverse) rang=angle_add(rang,180.0f);
if (angle_dif(rang,ang,&cwise)<90.0f) {
if (!obj->forward_move.reverse) {
obj->motion.ang.y=ang;
}
else {
obj->motion.ang.y=angle_add(ang,180.0f);
}
}
// reduce movement to slide against the walls
mz=*zadd/ws_step_factor;
mx=*xadd/ws_step_factor;
for (n=0;n!=ws_step_factor;n++) {
// try z then x movement first
xadd2=0;
yadd2=0;
zadd2=mz;
hit=collide_object_to_map(obj,&xadd2,&yadd2,&zadd2);
if (!hit) {
obj->pnt.z+=zadd2;
xadd2=mx;
yadd2=0;
zadd2=0;
if (!collide_object_to_map(obj,&xadd2,&yadd2,&zadd2)) {
obj->pnt.x+=xadd2;
}
continue;
}
// try x then z movement next
xadd2=mx;
yadd2=0;
zadd2=0;
hit=collide_object_to_map(obj,&xadd2,&yadd2,&zadd2);
if (!hit) {
obj->pnt.x+=xadd2;
xadd2=0;
yadd2=0;
zadd2=mz;
if (!collide_object_to_map(obj,&xadd2,&yadd2,&zadd2)) {
obj->pnt.z+=zadd2;
}
}
}
return(FALSE);
}
double vector_norm(struct vector* v) {
double norm_squared = vector_dot_product(v, v);
return sqrt(norm_squared);
}
uint8_t quest(vector v_B_c, vector v_sun_c, vector v_B_m, float sun_adc[], quaternion q_triad, uint8_t * p_w_ctrl)
{
uint8_t w_ctrl = *p_w_ctrl;
static uint16_t time_since_light;
static uint8_t light_prev = 1;
uint8_t light = 1, num_dark_sensors = 0, i, j;
vector v_sun_m, v_cross_m, v_cross_c, v_mc_cross, v_mc_add;
vector v_temp1, v_temp2;
vector v_triad;
float mu, nu, rho, k, triad;
for(i = 0; i < N_SS; i++)
{
if(sun_adc[i] < (0.5 * SS_GAIN))
num_dark_sensors++;
}
if(num_dark_sensors == N_SS)
light = 0;
if(light)
{
if(!w_ctrl)
{
time_since_light += FRAME_TIME;
if(time_since_light == 300)
w_ctrl = 0;
}
if(light_prev == 0)
{
w_ctrl = 0;
time_since_light = 0;
}
for(i = 0; i < (N_SS / 2); i++)
{
j = i * 2;
if(sun_adc[j] > sun_adc[j + 1])
v_sun_m[i] = (float)sun_adc[j];
else
v_sun_m[i] = -1 * (float)sun_adc[j + 1];
}
convert_unit_vector(v_sun_m);
/*v_B_m[0] = Current_state.mm.B_x;
v_B_m[1] = Current_state.mm.B_y;
v_B_m[2] = Current_state.mm.B_z;*/
vector_cross_product(v_B_m, v_sun_m, v_cross_m);
convert_unit_vector(v_cross_m);
vector_cross_product(v_B_c, v_sun_c, v_cross_c);
convert_unit_vector(v_cross_c);
mu = (1 + vector_dot_product(v_cross_m, v_cross_c)) * (MAG_WEIGHT * vector_dot_product(v_B_m, v_B_c) + (1 - MAG_WEIGHT) * vector_dot_product(v_sun_m, v_sun_c));
vector_cross_product(v_B_m, v_B_c, v_temp1);
vector_cross_product(v_sun_m, v_sun_c, v_temp2);
for(i = 0; i < 3; i++)
v_temp2[i] = v_temp1[i] * MAG_WEIGHT + (1 - MAG_WEIGHT) * v_temp2[i];
vector_cross_product(v_cross_m, v_cross_c, v_mc_cross);
mu += vector_dot_product(v_mc_cross, v_temp2);
add_vectors(v_cross_m, v_cross_c, v_mc_add);
nu = vector_dot_product(v_mc_add, v_temp2);
rho = sqrt(mu * mu + nu * nu);
if(mu > 0)
{
k = 1 / (2 * sqrt(rho * (rho + mu) * (1 + vector_dot_product(v_cross_m, v_cross_c))));
for(i = 0; i < 3; i++)
v_triad[i] = v_mc_cross[i] * (rho + mu) + v_mc_add[i] * nu;
triad = (rho + mu) * (1 + vector_dot_product(v_cross_m, v_cross_c));
}
else
{
k = 1 / (2 * sqrt(rho * (rho - mu) * (1 + vector_dot_product(v_cross_m, v_cross_c))));
for(i = 0; i < 3; i++)
v_triad[i] = v_mc_cross[i] * nu + v_mc_add[i] * (rho - mu);
triad = nu * (1 + vector_dot_product(v_cross_m, v_cross_c));
}
for(i = 0; i < 3; i++)
q_triad[i] = v_triad[i];
q_triad[3] = triad;
scalar_into_quaternion(q_triad, k);
}
else
{
for(i = 0; i < 3; i++)
q_triad[i] = 0;
q_triad[3] = 1;
}
light_prev = light;
return light;
}
