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 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);
}
void setup_orbit_vectors( ELEMENTS DLLPTR *e)
{
const double sin_incl = sin( e->incl), cos_incl = cos( e->incl);
double FAR *vec;
double vec_len;
double up[3];
unsigned i;
e->minor_to_major = sqrt( fabs( 1. - e->ecc * e->ecc));
e->lon_per = e->asc_node + atan2( sin( e->arg_per) * cos_incl,
cos( e->arg_per));
vec = e->perih_vec;
vec[0] = cos( e->lon_per) * cos_incl;
vec[1] = sin( e->lon_per) * cos_incl;
vec[2] = sin_incl * sin( e->lon_per - e->asc_node);
vec_len = sqrt( cos_incl * cos_incl + vec[2] * vec[2]);
if( cos_incl < 0.) /* for retrograde cases, make sure */
vec_len *= -1.; /* 'vec' has correct orientation */
for( i = 0; i < 3; i++)
vec[i] /= vec_len;
/* 'up' is a vector perpendicular to the plane of the orbit */
up[0] = sin( e->asc_node) * sin_incl;
up[1] = -cos( e->asc_node) * sin_incl;
up[2] = cos_incl;
vector_cross_product( e->sideways, up, vec);
}
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++;
}
}
static void fill_matrix( double mat[3][3], const ELEMENTS *elem)
{
memcpy( mat[0], elem->perih_vec, 3 * sizeof( double));
memcpy( mat[1], elem->sideways, 3 * sizeof( double));
/* mat[2] is the cross-product of mat[0] & mat[1]: */
vector_cross_product( mat[2], mat[0], mat[1]);
// mat[2][0] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
// mat[2][1] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
// mat[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
}
void rotate_vector_by_axis_small_angle(fixedpointnum *v1, fixedpointnum *axisvector, fixedpointnum angle) {
// rotates vector by small angle angle around axis vector.
fixedpointnum angleinradians=lib_fp_multiply(angle, FIXEDPOINTPIOVER180);
fixedpointnum crossproductvector[3];
vector_cross_product(axisvector,v1, crossproductvector);
v1[0]+=lib_fp_multiply(crossproductvector[0],angleinradians);
v1[1]+=lib_fp_multiply(crossproductvector[1],angleinradians);
v1[2]+=lib_fp_multiply(crossproductvector[2],angleinradians);
}
void eci2orbit(vector v_r, vector v_v, vector v_eci, vector v_orbit)
{
vector v_o_x, v_o_y, v_o_z; // x,y,z in orbit frame
uint8_t i;
vector_cross_product(v_v, v_r, v_o_y);
convert_unit_vector(v_o_y);
for(i = 0; i < 3; i++)
v_o_z[i] = -1 * v_r[i];
convert_unit_vector(v_o_z);
vector_cross_product(v_o_y, v_o_z, v_o_x);
convert_unit_vector(v_o_x);
matrix m_o = { { v_o_x[0], v_o_y[0], v_o_z[0] },
{ v_o_x[1], v_o_y[1], v_o_z[1] },
{ v_o_x[2], v_o_y[2], v_o_z[2] } };
vector_into_matrix(v_eci, m_o, v_orbit);
}
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);
}
void model_recalc_normals_mesh(model_type *model,int mesh_idx,bool only_tangent)
{
int n,k,t,neg_v;
float u10,u20,v10,v20,f_denom;
bool vertex_in_poly;
d3vct p10,p20,vlft,vrgt,v_num;
d3vct *normals,*nptr,*tangents,*tptr;
d3pnt *pt,*pt_1,*pt_2;
model_mesh_type *mesh;
model_vertex_type *vertex;
model_poly_type *poly;
mesh=&model->meshes[mesh_idx];
if ((mesh->nvertex==0) || (mesh->npoly==0)) return;
// memory for tangent space
normals=(d3vct*)malloc(mesh->npoly*sizeof(d3vct));
if (normals==NULL) return;
tangents=(d3vct*)malloc(mesh->npoly*sizeof(d3vct));
if (tangents==NULL) {
free(normals);
return;
}
// find tangent and binormal for triangles
poly=mesh->polys;
nptr=normals;
tptr=tangents;
for (n=0; n!=mesh->npoly; n++) {
// treat all polys as triangles
neg_v=poly->ptsz-1;
// get the side vectors (p1-p0) and (p2-p0)
pt=&mesh->vertexes[poly->v[0]].pnt;
pt_1=&mesh->vertexes[poly->v[1]].pnt;
pt_2=&mesh->vertexes[poly->v[neg_v]].pnt;
vector_create(&p10,pt_1->x,pt_1->y,pt_1->z,pt->x,pt->y,pt->z);
vector_create(&p20,pt_2->x,pt_2->y,pt_2->z,pt->x,pt->y,pt->z);
// calculate the normal by the cross
vector_cross_product(nptr,&p10,&p20);
nptr++;
// get the UV scalars (u1-u0), (u2-u0), (v1-v0), (v2-v0)
u10=poly->gx[1]-poly->gx[0];
u20=poly->gx[neg_v]-poly->gx[0];
v10=poly->gy[1]-poly->gy[0];
v20=poly->gy[neg_v]-poly->gy[0];
// calculate the tangent
// (v20xp10)-(v10xp20) / (u10*v20)-(v10*u20)
vector_scalar_multiply(&vlft,&p10,v20);
vector_scalar_multiply(&vrgt,&p20,v10);
vector_subtract(&v_num,&vlft,&vrgt);
f_denom=(u10*v20)-(v10*u20);
if (f_denom!=0.0f) f_denom=1.0f/f_denom;
vector_scalar_multiply(tptr,&v_num,f_denom);
vector_normalize(tptr);
tptr++;
poly++;
}
// average tangent space for each
// poly vertex to find one for shared
// vertexes
vertex=mesh->vertexes;
for (n=0; n!=mesh->nvertex; n++) {
vertex->tangent_space.tangent.x=vertex->tangent_space.tangent.y=vertex->tangent_space.tangent.z=0.0f;
vertex->tangent_space.normal.x=vertex->tangent_space.normal.y=vertex->tangent_space.normal.z=0.0f;
poly=mesh->polys;
nptr=normals;
tptr=tangents;
for (k=0; k!=mesh->npoly; k++) {
vertex_in_poly=FALSE;
for (t=0; t!=poly->ptsz; t++) {
if (poly->v[t]==n) {
vertex_in_poly=TRUE;
break;
}
}
if (vertex_in_poly) {
vertex->tangent_space.tangent.x+=tptr->x;
vertex->tangent_space.tangent.y+=tptr->y;
vertex->tangent_space.tangent.z+=tptr->z;
vertex->tangent_space.normal.x+=nptr->x;
vertex->tangent_space.normal.y+=nptr->y;
vertex->tangent_space.normal.z+=nptr->z;
}
poly++;
nptr++;
tptr++;
}
vector_normalize(&vertex->tangent_space.tangent);
if (!only_tangent) vector_normalize(&vertex->tangent_space.normal);
vertex++;
}
// free the tangent spaces
free(normals);
free(tangents);
// fix any normals that are 0,0,0
// this usually happens when there are bad
// polys
for (k=0; k!=mesh->nvertex; k++) {
if ((vertex->tangent_space.normal.x==0.0f) && (vertex->tangent_space.normal.y==0.0f) && (vertex->tangent_space.normal.z==0.0f)) {
pt=&mesh->vertexes[k].pnt;
vertex->tangent_space.normal.x=(float)(pt->x-vertex->pnt.x);
vertex->tangent_space.normal.y=(float)(pt->y-vertex->pnt.y);
vertex->tangent_space.normal.z=(float)(pt->z-vertex->pnt.z);
vector_normalize(&vertex->tangent_space.normal);
}
}
// determine in-out to map flips
// skip if not calculating normals
if (only_tangent) return;
// determine in/out and invert
vertex=mesh->vertexes;
for (n=0; n!=mesh->nvertex; n++) {
if (!model_recalc_normals_determine_vector_in_out(model,mesh_idx,n)) {
vertex->tangent_space.normal.x=-vertex->tangent_space.normal.x;
vertex->tangent_space.normal.y=-vertex->tangent_space.normal.y;
vertex->tangent_space.normal.z=-vertex->tangent_space.normal.z;
}
vertex++;
}
}
void model_draw_normals(int mesh_idx)
{
int n,nvertex;
float fx,fy,fz,vertexes[6];
float *pv,*pn,*pt;
d3vct tangent,normal,binormal;
model_mesh_type *mesh;
model_vertex_type *vertex;
// draw normals
glLineWidth(model_draw_normal_size);
mesh=&model.meshes[mesh_idx];
nvertex=mesh->nvertex;
vertex=mesh->vertexes;
pv=draw_setup.mesh_arrays[mesh_idx].gl_vertex_array;
pn=draw_setup.mesh_arrays[mesh_idx].gl_normal_array;
pt=draw_setup.mesh_arrays[mesh_idx].gl_tangent_array;
for (n=0;n!=nvertex;n++) {
// skip hiddens
if (model_vertex_mask_check_hide(mesh_idx,n)) {
pv+=3;
pn+=3;
pt+=3;
continue;
}
// vertex point
fx=*pv++;
fy=*pv++;
fz=*pv++;
normal.x=*pn++;
normal.y=*pn++;
normal.z=*pn++;
if (pref.show_tangent_binormal) {
// tangent
vertexes[0]=fx;
vertexes[1]=fy;
vertexes[2]=fz;
tangent.x=*pt++;
tangent.y=*pt++;
tangent.z=*pt++;
vertexes[3]=fx+(tangent.x*model_draw_normal_len);
vertexes[4]=fy+(tangent.y*model_draw_normal_len);
vertexes[5]=fz+(tangent.z*model_draw_normal_len);
glVertexPointer(3,GL_FLOAT,0,vertexes);
glColor4f(1.0f,0.0f,0.0f,1.0f);
glDrawArrays(GL_LINES,0,2);
// binormal
vector_cross_product(&binormal,&tangent,&normal);
vertexes[0]=fx;
vertexes[1]=fy;
vertexes[2]=fz;
vertexes[3]=fx+(binormal.x*model_draw_normal_len);
vertexes[4]=fy+(binormal.y*model_draw_normal_len);
vertexes[5]=fz+(binormal.z*model_draw_normal_len);
glVertexPointer(3,GL_FLOAT,0,vertexes);
glColor4f(0.0f,0.0f,1.0f,1.0f);
glDrawArrays(GL_LINES,0,2);
}
// normal
vertexes[0]=fx;
vertexes[1]=fy;
vertexes[2]=fz;
vertexes[3]=fx+(normal.x*model_draw_normal_len);
vertexes[4]=fy+(normal.y*model_draw_normal_len);
vertexes[5]=fz+(normal.z*model_draw_normal_len);
glVertexPointer(3,GL_FLOAT,0,vertexes);
glColor4f(1.0f,0.0f,1.0f,1.0f);
glDrawArrays(GL_LINES,0,2);
}
glLineWidth(1.0f);
}
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;
}
