// average pos and speed
void average_params() {
float n_pos = v_norm(pos_);
float n_sp = v_norm(speed_);
total_sum_pos = total_sum_pos - pos_buffer[avg_index] + n_pos;
total_sum_speed = total_sum_speed - speed_buffer[avg_index] + n_sp;
pos_buffer[avg_index] = n_pos;
speed_buffer[avg_index] = n_sp;
avg_pos = total_sum_pos / 4.0;
avg_speed = total_sum_speed / 4.0;
avg_index = (avg_index + 1) % 4;
}
void Remesh::selectClosest_tmp() {
int i=0;
FILE *select = fopen("select.dat", "w");
Vertex_handle vdst;
for (Vertex_iterator vi = dst_mesh.p.vertices_begin(); vi != dst_mesh.p.vertices_end(); vi++) {
vi->id = -1;
}
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
Kernel::Point_3 closest_point = dst_mesh.closestPoint(vi->point());
cerr << i << ": " << vi->point() << " closest to " << closest_point << endl;
if(v_norm(closest_point - vi->point()) < dst_mesh.edge_avg*alg_smoothing) {
vdst = dst_mesh.vertex_mapping[closest_point];
cerr << "whose id is " << vdst->id;
vdst->weight = 2;
vdst->id = i;
cerr << " changed to " << vdst->id << endl;
} i++;
}
i=0;
for (Vertex_iterator vi = dst_mesh.p.vertices_begin(); vi != dst_mesh.p.vertices_end(); vi++) {
if(vi->weight == 2) {
fprintf(select, "%d\t%d\n", i,vi->id);
}
i++;
}
fclose(select);
cerr << "Done with printing out stuff ! Exiting " << endl;
exit(0);
}
// init_scene initializes the global variable g_projectiles immediately after
// a shot has been fired.
void init_scene()
{
struct Tank *t = &g_tanks[g_current_tank];
Vector dir = {
sin(D2R(t->s.turret_angle))*cos(D2R(t->s.weapon_angle)),
sin(D2R(t->s.weapon_angle)),
cos(D2R(t->s.turret_angle))*cos(D2R(t->s.weapon_angle)),
};
Vector pos;
// FIX ME!! Is this the right way to handle tank angle?
// vv_add(dir, t->o.props.angular_position);
v_norm(dir);
// FIX ME!! We do not fire from the end of the barrel
vv_cpy(pos, t->o.props.position);
vv_add(pos, t->m.turret_base);
vv_add(pos, t->m.weapon_base);
printf("New projectile\n");
printf("Position: %f %f %f\n", pos[0], pos[1], pos[2]);
printf("Direction: %f %f %f\n", dir[0], dir[1], dir[2]);
printf("Tank pos: %f %f %f\n",
t->o.props.position[0], t->o.props.position[1], t->o.props.position[2]);
new_projectile(
dir, // Direction
t->s.power, // Magnitude
pos, // Position
Missile_I // Type
);
}
vector2d normalize(vector2d v) {
float norm = v_norm(v);
if( norm == 0.0) {
return v_mul(0.0, v);
} else {
return v_mul(1.0/norm, v);
}
}
static void orthonormalize(float dcm[3][3])
{
/* Orthogonalize the i and j unit vectors (DCMDraft2 Eqn. 19). */
dcm_err = v_dotp(dcm[0], dcm[1]);
v_scale(dcm[1], -dcm_err/2, dcm_d[0]); /* i vector correction */
v_scale(dcm[0], -dcm_err/2, dcm_d[1]); /* i vector correction */
v_add(dcm[0], dcm_d[0], dcm[0]);
v_add(dcm[1], dcm_d[1], dcm[1]);
/* k = i x j */
v_crossp(dcm[0], dcm[1], dcm[2]);
/* Normalize all three vectors. */
v_norm(dcm[0]);
v_norm(dcm[1]);
v_norm(dcm[2]);
}
// this is the method used in starlight aurora vsx, quite handy actually
void beginBlobs(vsx_module_engine_info* engine)
{
glEnable(GL_TEXTURE_2D);
GLfloat tmpMat[16];
glGetFloatv(GL_MODELVIEW_MATRIX, blobMat);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glGetFloatv(GL_PROJECTION_MATRIX, tmpMat);
tmpMat[3] = 0;
tmpMat[7] = 0;
tmpMat[11] = 0;
tmpMat[12] = 0;
tmpMat[13] = 0;
tmpMat[14] = 0;
tmpMat[15] = 1;
v_norm(tmpMat);
v_norm(tmpMat + 4);
v_norm(tmpMat + 8);
glLoadIdentity();
glMultMatrixf(tmpMat);
blobMat[3] = 0;
blobMat[7] = 0;
blobMat[11] = 0;
blobMat[12] = 0;
blobMat[13] = 0;
blobMat[14] = 0;
blobMat[15] = 1;
v_norm(blobMat);
v_norm(blobMat + 4);
v_norm(blobMat + 8);
glMultMatrixf(blobMat);
glGetFloatv(GL_PROJECTION_MATRIX, blobMat);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
//inv_mat(blobMat, blobMatInv);
GLfloat upLeft[] = {-0.5f, 0.5f, 0.0f, 1.0f};
GLfloat upRight[] = {0.5f, 0.5f, 0.0f, 1.0f};
mat_vec_mult(blobMat, upLeft, blobVec0);
mat_vec_mult(blobMat, upRight, blobVec1);
}
// normalizes ball position and returns normalized ball speed
void normalizeBallParams(int x_pos, int y_pos, uint sim_time) {
// position normalization
pos_.x = A_X*x_pos + B_X;
pos_.y = A_Y*y_pos + B_Y;
// normalizing pos
float n_pos = v_norm(pos_);
if(n_pos > 1.0) {
pos_.x /= n_pos;
pos_.y /= n_pos;
}
compute_speed(sim_time);
average_params();
}
// Actor neural network
int move(uint sim_time) {
int i = 0;
float theta = 0.0;
float psi = 0.0;
for(i = 0; i < N_MFM; i++) {
theta += w_A_theta_array[i] * phi_MFM_x(i);
psi += w_A_psi_array[i] * phi_MFM_y(i);
}
theta /= N_MFM;
psi /= N_MFM;
// range: [-1;1]
theta = theta + sigma_x() * n.x;
psi = psi + sigma_y() * n.y;
//io_printf(IO_BUF,"theta %f, psi %f\n", theta, psi);
theta = range(theta, -1.0, 1.0);
psi = range(psi, -1.0, 1.0);
if(sim_time > 300 && avg_pos < 0.15 && avg_speed < 0.03) {
sendNormMotorCommand(0.0, 0.0);
return STATE_BALANCED; // ball balanced
}
else if(sim_time % RESET_STEP == 0) {
// send an impulse from time to time
// if the ball is stucked on a place
if(v_norm(pos_) > 0.6){
sendNormMotorCommand(pos_.x, pos_.y);
}
}
else {
sendNormMotorCommand(theta, psi);
}
return STATE_UNBALANCED;
}
vector2d project(vector2d a, vector2d b) {
float coef = dot(a, b)/v_norm(b);
return v_mul(coef, normalize(b));
}
float Remesh::RunStep() {
cur_iter++;
// COMPUTE THE MESH DISPLACEMENT
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++)
vi->delta = Kernel::Vector_3(0,0,0);
// from current to the closest destination point
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
Kernel::Point_3 closest_point = dst_mesh.closestPoint(vi->point());
//cerr << "closest_point (" << closest_point << ") has ID " << dst_mesh.vertex_mapping[closest_point]->id << endl;
Kernel::Vector_3 closest_normal = dst_mesh.vertex_mapping[closest_point]->normal();
//Kernel::Vector_3 closest_normal = dst_mesh.vertexMapping(closest_point)->normal();
/*
vi->delta = closest_point - vi->point();
bool is_outside = v_norm((closest_point + closest_normal*v_norm(vi->delta)) - vi->point()) < v_norm(vi->delta);
// bool is_outside = v_norm(v_normalized(closest_normal) + v_normalized(vi->delta)) < 1.4142;
// bool is_outside = v_angle(closest_normal, vi->delta) > PI/2;
double dist_sign = (is_outside?1:-1);
vi->delta = vi->normal()*v_norm(vi->delta)*(-1)*dist_sign;
*/
vi->delta = vi->normal()* (closest_normal*(closest_point-vi->point()));
// vi->delta == v_normalized(data->mesh.computeVectorComponent(vi->normal(),vi->delta,1))*v_norm(vi->delta);
// vi->delta = v_normalized(vi->delta)*data->mesh.computeVertexStatistics(*vi,1)*0.05;
// vi->delta = vi->normal(); //*alg_dt
}
// NORMALIZE the movements
float total_movement = 0;
int total_elements = 0;
double max_delta = data->mesh.edge_avg*alg_dt;
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
//vi->delta = v_normalized(vi->delta)*data->mesh.computeVertexStatistics(*vi,1)*0.1;
// double max_delta = data->mesh.computeVertexStatistics(*vi,1)*alg_dt;
// double min_delta = data->mesh.edge_avg/5;
// vi->delta = vi->delta;
// vi->delta = vi->delta + v_normalized(vi->delta)*(RAND_MAX/2-std::rand())*1.0/RAND_MAX*data->mesh.edge_avg/2*alg_smoothing;
// vi->delta = v_normalized(vi->delta)*max_delta;
double the_norm = v_norm(vi->delta);
if (the_norm > max_delta) {
vi->delta = v_normalized(vi->delta)*max_delta;
}
// if (the_norm < min_delta) vi->delta = v_normalized(vi->delta)*min_delta;
the_norm = v_norm(vi->delta);
if (the_norm > max_delta*0.5) {
total_elements++;
total_movement += v_norm(vi->delta);
}
//vi->delta = vi->delta + Kernel::Vector_3((RAND_MAX/2-std::rand())*1.0/RAND_MAX, (RAND_MAX/2-std::rand())*1.0/RAND_MAX, (RAND_MAX/2-std::rand())*1.0/RAND_MAX)*data->mesh.computeVertexStatistics(*vi,1)*alg_smoothing;
// vi->delta = vi->delta + data->mesh.computeVectorComponent(vi->normal(),vi->laplacian()*alg_dt,0);
if (vi->border()==false) vi->delta = vi->delta + vi->laplacian()*alg_smoothing;
}
// MOVE THE MESH
OpenGLContext::mutex.lock();
data->mesh.lock();
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
if (alg_keepVerticesConstant) vi->delta = vi->normal()*(vi->delta*vi->normal());
vi->prev_delta = vi->delta;
vi->border()=false;
vi->move ( vi->delta );
}
data->mesh.unlock();
data->mesh.updateMeshData();
OpenGLContext::mutex.unlock();
//saveOutput
if (alg_saveOutput) {
char filename[300];
sprintf(filename,"%s/output_%04d.off",alg_saveOutputPrefix,cur_iter);
data->mesh.saveFormat(filename,"off");
sprintf(filename,"%s/output_%04d.diff",alg_saveOutputPrefix,cur_iter);
data->mesh.saveVectorField(filename);
sprintf(filename,"%s/output_%04d.idx",alg_saveOutputPrefix,cur_iter);
data->mesh.saveVertexIndices(filename);
}
emit stepFinished();
return total_elements;
// return (++cur_iter < iter);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
// run a step
float Remesh::RunColorStep() {
cur_iter++;
cerr << "ITERATION ( " << cur_iter << ") " << endl;
// COMPUTE THE MESH DISPLACEMENT
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
vi->delta = Kernel::Vector_3(0,0,0);
vi->delta_tmp = Kernel::Vector_3(0,0,0);
}
// from current to the closest destination point
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
#if MOVE_THE_MESH
Kernel::Point_3 closest_point = dst_mesh.closestColorPoint(vi->point(), (float *)(vi->color));
//Kernel::Point_3 closest_point = dst_mesh.closestPoint(vi->point());
#else
Kernel::Point_3 tmp_point = vi->point() + vi->motion_vec;
Kernel::Point_3 closest_point = dst_mesh.closestColorPoint(tmp_point, (float *)(vi->color));
#endif
//cerr << "closest_point (" << closest_point << ") has ID " << dst_mesh.vertex_mapping[closest_point]->id << endl;
//Kernel::Vector_3 closest_normal = dst_mesh.vertex_mapping[closest_point]->normal();
//Kernel::Vector_3 closest_normal = dst_mesh.vertexMapping(closest_point)->normal();
/*
vi->delta = closest_point - vi->point();
bool is_outside = v_norm((closest_point + closest_normal*v_norm(vi->delta)) - vi->point()) < v_norm(vi->delta);
// bool is_outside = v_norm(v_normalized(closest_normal) + v_normalized(vi->delta)) < 1.4142;
// bool is_outside = v_angle(closest_normal, vi->delta) > PI/2;
double dist_sign = (is_outside?1:-1);
vi->delta = vi->normal()*v_norm(vi->delta)*(-1)*dist_sign;
*/
vi->delta = closest_point- (vi->point() + vi->motion_vec);
//vi->delta = vi->normal()* (closest_normal*(closest_point-vi->point()));
// vi->delta == v_normalized(data->mesh.computeVectorComponent(vi->normal(),vi->delta,1))*v_norm(vi->delta);
// vi->delta = v_normalized(vi->delta)*data->mesh.computeVertexStatistics(*vi,1)*0.05;
// vi->delta = vi->normal(); //*alg_dt
}
// NORMALIZE the movements
float total_movement = 0;
int total_elements = 0;
double max_delta = data->mesh.edge_avg*alg_dt;
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
//vi->delta = v_normalized(vi->delta)*data->mesh.computeVertexStatistics(*vi,1)*0.1;
// double max_delta = data->mesh.computeVertexStatistics(*vi,1)*alg_dt;
// double min_delta = data->mesh.edge_avg/5;
// vi->delta = vi->delta;
// vi->delta = vi->delta + v_normalized(vi->delta)*(RAND_MAX/2-std::rand())*1.0/RAND_MAX*data->mesh.edge_avg/2*alg_smoothing;
// vi->delta = v_normalized(vi->delta)*max_delta;
double the_norm = v_norm(vi->delta);
if (the_norm > max_delta) {
vi->delta = v_normalized(vi->delta)*max_delta;
}
// if (the_norm < min_delta) vi->delta = v_normalized(vi->delta)*min_delta;
the_norm = v_norm(vi->delta);
if (! MOVE_THE_MESH || the_norm > max_delta*0.5) {
total_elements++;
total_movement += v_norm(vi->delta);
}
//vi->delta = vi->delta + Kernel::Vector_3((RAND_MAX/2-std::rand())*1.0/RAND_MAX, (RAND_MAX/2-std::rand())*1.0/RAND_MAX, (RAND_MAX/2-std::rand())*1.0/RAND_MAX)*data->mesh.computeVertexStatistics(*vi,1)*alg_smoothing;
// vi->delta = vi->delta + data->mesh.computeVectorComponent(vi->normal(),vi->laplacian()*alg_dt,0);
//vi->delta = vi->delta + vi->laplacian()*alg_smoothing;
}
data->mesh.diffuse(alg_smoothing, 0);
#if ADD_LENGTH_CONSTRAINT
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
// Add a cost for preserving the smoothness and the geometry of the mesh
HV_circulator h = vi->vertex_begin();
double geom_vx = 0 ;
double geom_vy = 0 ;
double geom_vz = 0 ;
int order=0;
do
{
const float xx = TO_FLOAT ( vi->point().x() );
const float yy = TO_FLOAT ( vi->point().y() );
const float zz = TO_FLOAT ( vi->point().z() );
const float xxx = TO_FLOAT ( h->opposite()->vertex()->point().x()) ;
const float yyy = TO_FLOAT ( h->opposite()->vertex()->point().y()) ;
const float zzz = TO_FLOAT ( h->opposite()->vertex()->point().z()) ;
double d = (xx - xxx) * (xx - xxx) + (yy - yyy) * (yy - yyy) + (zz - zzz) * (zz - zzz) ;
#if MOVE_THE_MESH
double t_d2x = (xx + vi->delta[0]) - (xxx + h->opposite()->vertex()->delta[0]) ;
double t_d2y = (yy + vi->delta[1]) - (yyy + h->opposite()->vertex()->delta[1]) ;
double t_d2z = (zz + vi->delta[2]) - (zzz + h->opposite()->vertex()->delta[2]) ;
#else
double t_d2x = (xx + vi->motion_vec[0] + vi->delta[0]) - (xxx + h->opposite()->vertex()->motion_vec[0] + h->opposite()->vertex()->delta[0]) ;
double t_d2y = (yy + vi->motion_vec[1] + vi->delta[1]) - (yyy + h->opposite()->vertex()->motion_vec[1] + h->opposite()->vertex()->delta[1]) ;
double t_d2z = (zz + vi->motion_vec[2] + vi->delta[2]) - (zzz + h->opposite()->vertex()->motion_vec[2] + h->opposite()->vertex()->delta[2]) ;
#endif
double d2 = t_d2x * t_d2x + t_d2y * t_d2y + t_d2z * t_d2z;
geom_vx += t_d2x * (sqrt(d2) - sqrt(d)) / sqrt(d2) ;
geom_vy += t_d2y * (sqrt(d2) - sqrt(d)) / sqrt(d2) ;
geom_vz += t_d2z * (sqrt(d2) - sqrt(d)) / sqrt(d2) ;
order++;
}
while ( ++h != vi->vertex_begin() );
geom_vx /= order ;
geom_vy /= order ;
geom_vz /= order ;
double alpha = 1 ;
vi->delta_tmp = alpha * Vector(-geom_vx, -geom_vy, -geom_vz) ;
#if 0
if(v_norm(vi->delta_tmp) > 0.001)
std::cout << vi->delta << " + " << geom_vx << " " << geom_vy << " " << geom_vz <<std::endl ;
#endif
}
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
double alpha1 = 0.9, alpha2 = 0.1 ;
if(v_norm(vi->delta)<0.01) { alpha2 = 0.9; alpha1 = 0.1; }
vi->delta = alpha1 * vi->delta + alpha2 * vi->delta_tmp;
}
#endif
// MOVE THE MESH
OpenGLContext::mutex.lock();
data->mesh.lock();
for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
if (alg_keepVerticesConstant) vi->delta = vi->normal()*(vi->delta*vi->normal());
vi->prev_delta = vi->delta;
#if MOVE_THE_MESH
vi->move ( vi->delta );
#else
vi->motion_vec = vi->motion_vec + vi->delta;
#endif
}
data->mesh.unlock();
#if MOVE_THE_MESH
data->mesh.updateMeshData();
#else
//for (Vertex_iterator vi = data->mesh.p.vertices_begin(); vi!=data->mesh.p.vertices_end(); vi++) {
// vi->motion_vec = vi->motion_vec + vi->laplacian() * alg_smoothing; // smooth the motion vectors
//}
#endif
OpenGLContext::mutex.unlock();
//saveOutput
if (alg_saveOutput) {
char filename[300];
sprintf(filename,"%s/output_%04d.off",alg_saveOutputPrefix,cur_iter);
data->mesh.saveFormat(filename,"off");
sprintf(filename,"%s/output_%04d.diff",alg_saveOutputPrefix,cur_iter);
data->mesh.saveVectorField(filename);
sprintf(filename,"%s/output_%04d.idx",alg_saveOutputPrefix,cur_iter);
data->mesh.saveVertexIndices(filename);
}
emit stepFinished();
return total_movement;
//return total_elements;
// return (++cur_iter < iter);
}
void update_ahrs(float dt, float dcm_out[3][3], float gyr_out[3])
{
static uint8_t i;
read_mpu(v_gyr, v_acc);
for (i=0; i<3; i++) {
gyr_out[i] = v_gyr[i];
}
/**
* Accelerometer
* Frame of reference: body
* Units: G (gravitational acceleration)
* Purpose: Measure the acceleration vector v_acc with components
* codirectional with the i, j, and k vectors. Note that the
* gravitational vector is the negative of the K vector.
*/
#ifdef ACC_WEIGHT
// Take weighted average.
#ifdef ACC_SELF_WEIGHT
for (i=0; i<3; i++) {
v_acc[i] = ACC_SELF_WEIGHT * v_acc[i] + (1-ACC_SELF_WEIGHT) * v_acc_last[i];
v_acc_last[i] = v_acc[i];
// Kalman filtering?
//v_acc_last[i] = acc.get(i);
//kalmanUpdate(v_acc[i], accVar, v_acc_last[i], ACC_UPDATE_SIG);
//kalmanPredict(v_acc[i], accVar, 0.0, ACC_PREDICT_SIG);
}
#endif // ACC_SELF_WEIGHT
acc_scale = v_norm(v_acc);
/**
* Reduce accelerometer weight if the magnitude of the measured
* acceleration is significantly greater than or less than 1 g.
*
* TODO: Magnitude of acceleration should be reported over telemetry so
* ACC_SCALE_WEIGHT can be more accurately determined.
*/
#ifdef ACC_SCALE_WEIGHT
acc_scale = (1.0 - MIN(1.0, ACC_SCALE_WEIGHT * ABS(acc_scale - 1.0)));
acc_weight = ACC_WEIGHT * acc_scale;
#else
acc_weight = ACC_WEIGHT;
// Ignore accelerometer if it measures anything 0.5g past gravity.
if (acc_scale > 1.5 || acc_scale < 0.5) {
acc_weight = 0;
}
#endif // ACC_SCALE_WEIGHT
/**
* Express K global unit vector in body frame as k_gb for use in drift
* correction (we need K to be described in the body frame because gravity
* is measured by the accelerometer in the body frame). Technically we
* could just create a transpose of dcm_gyro, but since we don't (yet) have
* a magnetometer, we don't need the first two rows of the transpose. This
* saves a few clock cycles.
*/
for (i=0; i<3; i++) {
k_gb[i] = dcm_gyro[i][2];
}
/**
* Calculate gyro drift correction rotation vector w_a, which will be used
* later to bring KB closer to the gravity vector (i.e., the negative of
* the K vector). Although we do not explicitly negate the gravity vector,
* the cross product below produces a rotation vector that we can later add
* to the angular displacement vector to correct for gyro drift in the
* X and Y axes. Note we negate w_a because our acceleration vector is
* actually the negative of our gravity vector.
*/
v_crossp(k_gb, v_acc, w_a);
v_scale(w_a, -1, w_a);
#endif // ACC_WEIGHT
/**
* Magnetometer
* Frame of reference: body
* Units: N/A
* Purpose: Measure the magnetic north vector v_mag with components
* codirectional with the body's i, j, and k vectors.
*/
#ifdef MAG_WEIGHT
// Express J global unit vectory in body frame as j_gb.
for (i=0; i<3; i++) {
j_gb[i] = dcm_gyro[i][1];
}
// Calculate yaw drift correction vector w_m.
v_crossp(j_gb, v_mag, w_m);
#endif // MAG_WEIGHT
/**
* Gyroscope
* Frame of reference: body
* Units: rad/s
* Purpose: Measure the rotation rate of the body about the body's i,
* j, and k axes.
* ========================================================================
* Scale v_gyr by elapsed time (in seconds) to get angle w*dt in radians,
* then compute weighted average with the accelerometer and magnetometer
* correction vectors to obtain final w*dt.
* TODO: This is still not exactly correct.
*/
for (i=0; i<3; i++) {
w_dt[i] = v_gyr[i] * dt;
#ifdef ACC_WEIGHT
w_dt[i] += acc_weight * w_a[i];
#endif // ACC_WEIGHT
#ifdef MAG_WEIGHT
w_dt[i] += MAG_WEIGHT * w_m[i];
#endif // MAG_WEIGHT
}
/**
* Direction Cosine Matrix
* Frame of reference: global
* Units: None (unit vectors)
* Purpose: Calculate the components of the body's i, j, and k unit
* vectors in the global frame of reference.
* ========================================================================
* Skew the rotation vector and sum appropriate components by combining the
* skew symmetric matrix with the identity matrix. The math can be
* summarized as follows:
*
* All of this is calculated in the body frame. If w is the angular
* velocity vector, let w_dt (w*dt) be the angular displacement vector of
* the DCM over a time interval dt. Let w_dt_i, w_dt_j, and w_dt_k be the
* components of w_dt codirectional with the i, j, and k unit vectors,
* respectively. Also, let dr be the linear displacement vector of the DCM
* and dr_i, dr_j, and dr_k once again be the i, j, and k components,
* respectively.
*
* In very small dt, certain vectors approach orthogonality, so we can
* assume that (draw this out for yourself!):
*
* dr_x = < 0, dw_k, -dw_j>,
* dr_y = <-dw_k, 0, dw_i>, and
* dr_z = < dw_j, -dw_i, 0>,
*
* which can be expressed as the rotation matrix:
*
* [ 0 dw_k -dw_j ]
* dr = [ -dw_k 0 dw_i ]
* [ dw_j -dw_i 0 ].
*
* This can then be multiplied by the current DCM and added to the current
* DCM to update the DCM. To minimize the number of calculations performed
* by the processor, however, we can combine the last two steps by
* combining dr with the identity matrix to produce:
*
* [ 1 dw_k -dw_j ]
* dr + I = [ -dw_k 1 dw_i ]
* [ dw_j -dw_i 1 ],
*
* which we multiply with the current DCM to produce the updated DCM
* directly.
*
* It may be helpful to read the Wikipedia pages on cross products and
* rotation representation.
*/
dcm_d[0][0] = 1;
dcm_d[0][1] = w_dt[2];
dcm_d[0][2] = -w_dt[1];
dcm_d[1][0] = -w_dt[2];
dcm_d[1][1] = 1;
dcm_d[1][2] = w_dt[0];
dcm_d[2][0] = w_dt[1];
dcm_d[2][1] = -w_dt[0];
dcm_d[2][2] = 1;
// Multiply the current DCM with the change in DCM and update.
m_product(dcm_d, dcm_gyro, dcm_gyro);
// Remove any distortions introduced by the small-angle approximations.
orthonormalize(dcm_gyro);
// Apply rotational offset (in case IMU is not installed orthogonally to
// the airframe).
dcm_offset[0][0] = 1;
dcm_offset[0][1] = 0;
dcm_offset[0][2] = -trim_angle[1];
dcm_offset[1][0] = 0;
dcm_offset[1][1] = 1;
dcm_offset[1][2] = trim_angle[0];
dcm_offset[2][0] = trim_angle[1];
dcm_offset[2][1] = -trim_angle[0];
dcm_offset[2][2] = 1;
m_product(dcm_offset, dcm_gyro, dcm_out);
}
int getGeoDataTria( double *p1, double *p2, double *p3, double *Ix, double *Iy, double *Ixy,
double *A, double *pcg)
/* p3 */
/* Tri_2 /|\ Tri_1 */
/* p1_p2 */
/* (p4) */
{
int i;
static double vx[]={1.,0.,0.};
double p4[3], v12[3], v13[3], v23[3], v14[3], v43[3], vnorm[3], vbuf[3], vbuf2[3];
double p_puf[3][3], vb; /* pktnr,xyz */
double l12, l13, l23, b1, b2, b_ges, h, A1, A2;
double Ix1_p4, Iy1_p4, Ixy1_p4;
double Ix2_p4, Iy2_p4, Ixy2_p4;
double Ix_p4, Iy_p4, Ixy_p4;
double spx_p4, spy_p4, sp1x_p4, sp2x_p4;
double Ix_sp, Iy_sp, Ixy_sp;
double alfa_z, a, b, c;
/* v12 muss den groeste Laenge haben, sonst umsortieren und checken ob es ein dreieck ist v12xv13 != 0. */
v_result(p1, p2, v12);
l12=v_betrag(v12);
v_result(p1, p3, v13);
v_prod(v12,v13,vbuf);
if((int)(1.e20*v_betrag(vbuf))==0) return(0);
l13=v_betrag(v13);
v_result(p2, p3, v23);
l23=v_betrag(v23);
vb=l12;
if (l13 > vb)
{
vb=l13;
for (i=0; i<3; i++) p_puf[0][i]= p3[i];
for (i=0; i<3; i++) p_puf[1][i]= p1[i];
for (i=0; i<3; i++) p_puf[2][i]= p2[i];
for (i=0; i<3; i++) p1[i]=p_puf[0][i];
for (i=0; i<3; i++) p2[i]=p_puf[1][i];
for (i=0; i<3; i++) p3[i]=p_puf[2][i];
}
if (l23 > vb)
{
vb=l23;
for (i=0; i<3; i++) p_puf[0][i]= p2[i];
for (i=0; i<3; i++) p_puf[1][i]= p3[i];
for (i=0; i<3; i++) p_puf[2][i]= p1[i];
for (i=0; i<3; i++) p1[i]=p_puf[0][i];
for (i=0; i<3; i++) p2[i]=p_puf[1][i];
for (i=0; i<3; i++) p3[i]=p_puf[2][i];
}
/* berechnung der Geometiegroessen (2D-Elementsys.) aus 3D Punktkoordinaten*/
v_result(p1, p3, v13);
v_result(p1, p2, v12);
b_ges=v_betrag(v12);
b2=v_sprod( v13, v12);
b2=b2/b_ges;
b1=b_ges-b2;
v_norm( v12, vnorm);
v_scal( &b2, vnorm, v14);
v_add( p1, v14, p4);
v_result(p4, p3, v43);
h=v_betrag(v43);
A1= b1*h/2.;
*A= b_ges*h/2.;
A2= *A-A1;
/* Schwerpunkte (sp) bezogen auf p4 IM LOKALEN SYSTEM */
sp1x_p4= b1/3.;
sp2x_p4= -b2/3.;
spx_p4 = (A1*sp1x_p4 + A2*sp2x_p4) / *A;
spy_p4 = h/3.;
/* Traegheitsmomente bezogen auf p4 IM LOKALEN SYSTEM */
Ix1_p4= b1*h*h*h/12.;
Iy1_p4= h*b1*b1*b1/12.;
Ixy1_p4= b1*b1*h*h/24.;
Ix2_p4= b2*h*h*h/12.;
Iy2_p4= h*b2*b2*b2/12.;
Ixy2_p4= b2*b2*h*h/24.;
Ix_p4= Ix1_p4 + Ix2_p4;
Iy_p4= Iy1_p4 + Iy2_p4;
Ixy_p4= Ixy1_p4 - Ixy2_p4;
/*
printf ("\nA:%lf b1:%lf b2:%lf h:%lf \n", *A, b1, b2, h);
printf ("Tri1_P4 Ix:%lf Iy:%lf Ixy:%lf \n", Ix1_p4, Iy1_p4, Ixy1_p4);
printf ("Tri2_P4 Ix:%lf Iy:%lf Ixy:%lf \n", Ix2_p4, Iy2_p4, Ixy2_p4);
printf ("Tri _P4 Ix:%lf Iy:%lf Ixy:%lf \n", Ix_p4, Iy_p4, Ixy_p4);
*/
/* Traegheitsmomente bezogen auf Schwp. (nach Steiner) */
Ix_sp = Ix_p4 - spy_p4*spy_p4 * *A;
Iy_sp = Iy_p4 - spx_p4*spx_p4 * *A;
Ixy_sp = Ixy_p4 - spx_p4*spy_p4 * *A;
/*
printf ("\nA:%lf b1:%lf b2:%lf h:%lf \n", *A, b1, b2, h);
printf ("sp_p4 spx:%lf spy:%lf \n", spx_p4, spy_p4);
printf ("Tri _sp Ix:%lf Iy:%lf Ixy:%lf \n", Ix_sp, Iy_sp, Ixy_sp);
*/
/* umrechnen aufs Gloabale System (2D Elementsystem -> 3D Globalsystem) */
v_norm( v12, vnorm);
v_scal( &spx_p4, vnorm, vbuf);
v_add( p4, vbuf, vbuf2);
v_norm( v43, vnorm);
v_scal( &spy_p4, vnorm, vbuf);
v_add( vbuf2, vbuf, pcg);
/* ACHTUNG: z. Z. wird nur in der xy-ebene gedreht! */
v12[2] = 0.;
alfa_z= acos( v_sprod(v12,vx) / v_betrag(v12) / v_betrag(vx) ) * (-1.);
a = (Ix_sp+Iy_sp)/2.;
b = (Ix_sp-Iy_sp)/2. * cos(2.*alfa_z);
c = Ixy_sp * sin(2.*alfa_z);
*Ix= a+b-c;
*Iy= a-b+c;
*Ixy= a * sin(2.*alfa_z) + Ixy_sp * cos(2.*alfa_z);
/* Traegheitsmomente bezogen auf Globalsystem (nach Steiner) */
*Ix= *Ix + pcg[1]*pcg[1] * *A;
*Iy= *Iy + pcg[0]*pcg[0] * *A;
*Ixy= *Ixy + pcg[0]*pcg[1] * *A;
return(1);
}
/* Third version of draw triangle uses a z_buffer
And we're using flat shading
clear_z_buffer needs to be run in the main loop before every frame. */
void draw_triangle3(int *a,int *b,int *c,vect_t pos,vect_t rot,rgb color){
int x0,y0,x1,y1,x2,y2;
// this is is the format that the triangles should be passed to us already
vect_t x={a[0],a[1],a[2]+Z};
vect_t y={b[0],b[1],b[2]+Z};
vect_t z={c[0],c[1],c[2]+Z};
// Lighting
vect_t n=v_norm(v_cross(x,y));
// vect_t l=v_sub(cam.pos,x);
vect_t l=cam.pos;
double lum=1.0;
// if( lum < 0 ) return 0;
rgb colors[3];
point p[3];
x=v_rot(x,rot);
y=v_rot(y,rot);
z=v_rot(z,rot);
// should we do the translation here as well does this make sense?
x=v_add(x,pos);
y=v_add(y,pos);
z=v_add(z,pos);
double lums[3];
lums[0]=v_dot(l,x)/(v_len(l)*v_len(x));
lums[1]=v_dot(l,y)/(v_len(l)*v_len(y));
lums[2]=v_dot(l,z)/(v_len(l)*v_len(z));
#if 1
for(int i=0;i<3;i++){
#if 0
lum=lums[i];
if( lum < 0 ) lum=0;
#else
lum=1.0;
#endif
colors[i].r=clip(color.r*lum);
colors[i].g=clip(color.g*lum);
colors[i].b=clip(color.b*lum);
}
#else
for(int i=0;i<3;i++){
colors[i].r=color.r;
colors[i].g=color.g;
colors[i].b=color.b;
}
#endif
int q,w,e;
q=project_coord(x,&p[0].x,&p[0].y);
w=project_coord(y,&p[1].x,&p[1].y);
e=project_coord(z,&p[2].x,&p[2].y);
if(outside_of_screen(p)){
return;
}
rgb ca[3];
ca[0]=ca[1]=ca[2]=color;
vect_t v3[3];
v3[0]=x; v3[1]=y; v3[2]=z;
double distances[3];
// introduce max distance of 5000
distances[0]=clipfloat(v_len(v_sub(x,cam.pos)),0,MAX_DIST)/MAX_DIST;
distances[1]=clipfloat(v_len(v_sub(y,cam.pos)),0,MAX_DIST)/MAX_DIST;
distances[2]=clipfloat(v_len(v_sub(z,cam.pos)),0,MAX_DIST)/MAX_DIST;
/*
distances[0]=v_len(v_sub(z,cam.pos));
distances[1]=v_len(v_sub(y,cam.pos));
distances[2]=v_len(v_sub(x,cam.pos));
*/
draw_triangle_gradient_new(p,v3,distances,colors);
#if 0
draw_line_color(p[0].x,p[0].y,p[1].x,p[1].y,black);
draw_line_color(p[1].x,p[1].y,p[2].x,p[2].y,black);
draw_line_color(p[1].x,p[1].y,p[0].x,p[0].y,black);
#endif
}
int draw_3d_tri(
pVector v1p, Graph3dColor c1p, pVector n1p,
pVector v2p, Graph3dColor c2p, pVector n2p,
pVector v3p, Graph3dColor c3p, pVector n3p) {
int j;
int x1, x2, x3;
int y1, y2, y3;
// First, convert the colors to floats
Vector c1 = {
(float)c1p.r / 255.0,
(float)c1p.g / 255.0,
(float)c1p.b / 255.0,
1.0
};
Vector c2 = {
(float)c2p.r / 255.0,
(float)c2p.g / 255.0,
(float)c2p.b / 255.0,
1.0
};
Vector c3 = {
(float)c3p.r / 255.0,
(float)c3p.g / 255.0,
(float)c3p.b / 255.0,
1.0
};
Vector v1, v2, v3;
Vector n1, n2, n3;
Vector L1, L2, L3;
// Next, apply the modelview matrix to the triangle's vertices
vv_cpy(v1, v1p); vv_cpy(v2, v2p); vv_cpy(v3, v3p);
vm_mul(v1, modelview);
vm_mul(v2, modelview);
vm_mul(v3, modelview);
// And also to the normals
vv_cpy(n1, n1p); vv_cpy(n2, n2p); vv_cpy(n3, n3p);
vm_mul(n1, modelview);
vm_mul(n2, modelview);
vm_mul(n3, modelview);
// Find the distance for each point from the light
vv_cpy(L1, lightpos); vv_sub(L1, v1); v_norm(L1);
vv_cpy(L2, lightpos); vv_sub(L2, v2); v_norm(L2);
vv_cpy(L3, lightpos); vv_sub(L3, v3); v_norm(L3);
// Then, apply our lighting equations
// We assume that the triangle's ambient and diffuse values are the same
// We also assume that
for(j = 0; j < 4; j++) {
c1[j] = light_transform(c1[j], n1, L1, j);
c2[j] = light_transform(c2[j], n2, L2, j);
c2[j] = light_transform(c3[j], n3, L3, j);
}
// Convert the color values to values the graphics card will accept
// c1p et al were passed by value, so we can modify them
c1p.r = c1[0] * 255;
c1p.g = c1[1] * 255;
c1p.b = c1[2] * 255;
c2p.r = c2[0] * 255;
c2p.g = c2[1] * 255;
c2p.b = c2[2] * 255;
c3p.r = c3[0] * 255;
c3p.g = c3[1] * 255;
c3p.b = c3[2] * 255;
// Lastly, find the coordinates on the screen for each point,
// and draw the triangle
x1 = 800 * (depth/height) * (v1[0] / v1[2]);
y1 = 600 * (depth/height) * (v1[1] / v1[2]);
x2 = 800 * (depth/height) * (v2[0] / v2[2]);
y2 = 600 * (depth/height) * (v2[1] / v2[2]);
x3 = 800 * (depth/height) * (v3[0] / v3[2]);
y3 = 600 * (depth/height) * (v3[1] / v3[2]);
draw_2d_tri(x1, y1, c1p, x2, y2, c2p, x3, y3, c3p);
printf("v1: %f %f %f\n", v1[0], v1[1], v1[2]);
printf("v2: %f %f %f\n", v2[0], v2[1], v2[2]);
printf("v2: %f %f %f\n", v3[0], v3[1], v3[2]);
printf("%d %d; %d %d; %d %d\n", x1, y1, x2, y2, x3, y3);
return GRAPH3D_OK;
}
