void CScene3D::m_TranslateCamera(float4 dx, float4 dy, float4 dz)
{
AD_Vect3D x, y, z, sum;
AD_Vect3D ptmp;
AD_Matrix pretrans;
if (!p_CurrentCamera) return;
mat_get_row(&p_CurrentCamera->p_WorldMatrix, 0, &x);
mat_get_row(&p_CurrentCamera->p_WorldMatrix, 1, &y);
mat_get_row(&p_CurrentCamera->p_WorldMatrix, 2, &z);
vect_auto_scale(&x, dx);
vect_auto_scale(&y, dy);
vect_auto_scale(&z, dz);
vect_add(&x, &y, &sum);
vect_auto_add(&sum, &z);
vect_auto_add(&p_CurrentCamera->p_CurrentPosition, &sum);
vect_auto_add(&p_CurrentCamera->p_CurrentTargetPosition, &sum);
vect_neg(&p_CurrentCamera->p_CurrentPosition, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
mat_mul(&p_CurrentCamera->p_CurrentRotationMatrix,
&pretrans,
&p_CurrentCamera->p_WorldMatrix);
}
void
fmt_int_sign_64
(t_vect *a
, int base
, long x)
{
bool neg;
char s[20];
size_t i;
i = 19;
neg = (x < 0);
if (!x)
return ((void)vect_mset_end(a, '0', 1));
while ((x > 0 || x <= base) && ((x < 0) || (x >= base)))
{
s[i] = x % base;
s[i] = ABS(s[i]);
s[i] += (s[i] <= 9) ? '0' : 'a' - 10;
x /= base;
i--;
}
if (x)
s[i--] = ABS(x) + (x <= 9 ? '0' : 'a' - 10);
if (neg)
s[i--] = '-';
i++;
vect_add(a, s + i, 20 - i);
}
void torque(BEMRI * b, double * t)
{
double F1[3] = {(X3-X1),(Y3-Y1),(Z3-Z1)} , F2[3] = {(X3-X2),(Y3-Y2),(Z3-Z2)};
double r1h = sqrt(pow((X3-X1),2) + pow((Y3-Y1),2) + pow((Z3-Z1),2));
double r2h = sqrt(pow((X3-X2),2) + pow((Y3-Y2),2) + pow((Z3-Z2),2));
double r1cm[3] = {(X4-X1),(Y4-Y1),(Z4-Z1)} , r2cm[3] = {(X4-X2),(Y4-Y2),(Z4-Z2)};
double t1[3], t2[3];
for(int ii = 0; ii < 3; ii++)
{
F1[ii] *= G*m1*m3/pow(r1h,3); //compute forces on each component from SMBH
F2[ii] *= G*m2*m3/pow(r2h,3);
}
cross_prod(r1cm, F1, t1); //compute the torque from R cross F
cross_prod(r2cm, F2, t2);
vect_add(t1, t2, t, 3); //add the individual torques and store in t
/*
printf("\ntorque is:");
Vdisplay(t,3);
Vdisplay(F1,3);
Vdisplay(r1cm,3);
Vdisplay(F2,3);
Vdisplay(r2cm,3);
anykey();
*/
}
void CCamera::m_Ray (float4 screenX, float4 screenY, int32 space, Ray *out)
{
// screenX e screenY vanno nel range [-1;1]
AD_Vect3D cam_x, cam_y, cam_z;
float4 fx, fy;
if (!out) return;
fx=(float4)tan(p_CurrentFov*0.5);
fy=(float4)tan(p_CurrentFov*0.5);
if (space==WORLDSPACE_RAY)
{
vect_copy(&p_CurrentPosition, &out->base);
mat_get_row(&p_CurrentRotationMatrix, 0, &cam_x);
mat_get_row(&p_CurrentRotationMatrix, 1, &cam_y);
mat_get_row(&p_CurrentRotationMatrix, 2, &cam_z);
vect_auto_scale(&cam_x, screenX*fx);
vect_auto_scale(&cam_y, screenY*fy*480.0f/640.0f);
vect_add(&cam_x, &cam_y, &out->direction);
vect_auto_add(&out->direction, &cam_z);
vect_auto_normalize(&out->direction);
}
else
if (space==CAMERASPACE_RAY)
{
vect_set(&out->base, 0, 0, 0);
vect_set(&out->direction, screenX*fx, screenY*fy, 1);
}
vect_auto_normalize(&out->direction);
}
t_vect *get_normal_at_cylinder(t_cylinder *c, t_vect *point)
{
t_vect *v;
v = normalize(vect_add(new_vector(point->x, 0, point->z),
negative(new_vector(c->center->x, 0, c->center->z))));
return (v);
}
void gen_vertex_normals(MESH *m)
{
int i;
LLIST *vert = malloc(sizeof(LLIST)*m->nv);
memset(vert, 0, sizeof(LLIST)*m->nv);
LLIST *tmp;
int index;
// tell each vert about it's faces
for(i=0; i<m->nf; i++)
{
index = m->f[i].x;
tmp = malloc(sizeof(LLIST));
tmp->face = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].y;
tmp = malloc(sizeof(LLIST));
tmp->face = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].z;
tmp = malloc(sizeof(LLIST));
tmp->face = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
}
float3 *no = malloc(sizeof(float3)*m->nv); // normal out
float3 t;
// now average the normals and store in the output
for(i=0; i<m->nv; i++)
{
tmp = vert[i].next;
t.x = t.y = t.z = 0;
while(tmp)
{
vect_add( &t, &t, &m->n[tmp->face]);
LLIST *last = tmp;
tmp = tmp->next;
free(last);
}
vect_norm(&no[i], &t);
}
free(vert);
free(m->n);
m->n = no;
m->nn = m->nv;
}
void calc_total_angular_momentum(BEMRI * b)
{
double p1[3],p2[3],p3[3],l1[3],l2[3],l3[3],temp[3];
(*b).L = 0;
vect_mult_scalar(V1,m1,p1,3);
vect_mult_scalar(V2,m2,p2,3);
vect_mult_scalar(V3,m3,p3,3);
cross_prod(R1,p1,l1);
cross_prod(R2,p2,l2);
cross_prod(R3,p3,l3);
vect_add(l1,l2,temp,3);
vect_add(l3,temp,temp,3);
(*b).L = mag(temp,3);
return;
}
void CWindModifier::m_Force (float4 framepos, AD_Vect3D *pos, AD_Vect3D *vel, AD_Vect3D *accel)
{
AD_Vect3D posrel, tf, p, force, p2;
float4 dist, freq, turb;
freq=p_Frequency;
turb=p_Turbolence;
if (p_Mapping==FORCE_PLANAR)
{
if (p_Decay!=0.0f)
{
vect_sub(pos, &p_CurrentPosition, &posrel);
dist=fabsf(vect_dot(&p_Force, &posrel));
vect_scale(&p_ScaledForce, (float4)exp(-p_Decay*dist), accel);
}
else vect_copy(&p_ScaledForce, accel);
}
else
{
vect_sub(pos, &p_CurrentPosition, &force);
dist = vect_length(&force);
if (dist != 0) vect_auto_scale(&force, 1.0f/dist);
if (p_Decay != 0)
vect_scale(&force, p_ScaledStrength*(float4)exp(-p_Decay*dist), accel);
else
vect_scale(&force, p_ScaledStrength, accel);
}
if (turb != 0)
{
vect_sub(pos, &p_CurrentPosition, &p2);
freq *= 0.01f;
vect_copy(&p2, &p);
p.x = freq * framepos;
tf.x = noise3(p.x*p_Scale, p.y*p_Scale, p.z*p_Scale);
vect_copy(&p2, &p);
p.y = freq * framepos;
tf.y = noise3(p.x*p_Scale, p.y*p_Scale, p.z*p_Scale);
vect_copy(&p2, &p);
p.z = freq * framepos;
tf.z = noise3(p.x*p_Scale, p.y*p_Scale, p.z*p_Scale);
turb *= 0.0001f*forceScaleFactor;
vect_auto_scale(&tf, turb);
vect_add(accel, &tf, accel);
}
}
int true_anomaly(binary * b)
{
double x,y,z,vx,vy,vz,r,v,k,h[3],R[3],V[3],e_vec[3],vxh1[3],vxh2[3],R_temp[3],theta,edotr;
int rv=1;
//First translate to mass 1 rest frame
x = b->x2[0] - b->x1[0];
y = b->x2[1] - b->x1[1];
z = b->x2[2] - b->x1[2];
vx = b->v2[0] - b->v1[0];
vy = b->v2[1] - b->v1[1];
vz = b->v2[2] - b->v1[2];
r = sqrt(x*x + y*y + z*z);
v = sqrt(vx*vx + vy*vy + vz*vz);
k = G*b->mass1 + G*b->mass2; //here k is the standard gravitational parameter G(m1 + m2)
//put together vectors of r and v
R[0] = x;
R[1] = y;
R[2] = z;
V[0] = vx;
V[1] = vy;
V[2] = vz;
//calculate vector h
cross_prod(R,V,h);
//calculate the eccentricity vector
cross_prod(V,h,vxh1);
vect_mult_scalar(vxh1, 1/k, vxh2, 3);
vect_mult_scalar(R,-1/r,R_temp,3);
vect_add(vxh2,R_temp,e_vec,3);
//find true anomaly from r
edotr = dot_prod(e_vec,R,3);
theta = acos(edotr / ( mag(e_vec,3) * mag(R,3) ) );
if(dot_prod(R,V,3) < 0)
{
rv = 0;
//printf("\nold theta = %.3f\nnew theta = %.3f\n",theta,2*PI-theta);
theta = 2*PI - theta;
}
b->true_anomaly = theta;
return rv;
}
void gen_vertex_normals(MESH *m)
{
ListInt *vert = malloc(sizeof(ListInt)*m->nv);
memset(vert, 0, sizeof(ListInt)*m->nv);
// tell each vert about it's faces
for(int i=0; i<m->nf; i++)
{
int index = m->f[i].x;
ListInt *tmp = malloc(sizeof(ListInt));
tmp->x = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].y;
tmp = malloc(sizeof(ListInt));
tmp->x = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
index = m->f[i].z;
tmp = malloc(sizeof(ListInt));
tmp->x = i;
tmp->next = vert[index].next;
vert[index].next = tmp;
}
float3 *no = malloc(sizeof(float3)*m->nv); // normal out
// now average the normals and store in the output
for(int i=0; i<m->nv; i++)
{
ListInt *tmp = vert[i].next;
float3 t = { 0, 0, 0 };
while(tmp)
{
vect_add( &t, &t, &m->n[tmp->x]);
ListInt *last = tmp;
tmp = tmp->next;
free(last);
}
vect_norm(&no[i], &t);
}
free(vert);
free(m->n);
m->n = no;
m->nn = m->nv;
}
void CScene3D::m_RotateCamera(float4 ax, float4 ay, float4 az)
{
AD_Vect3D cameradir, xAxis, yAxis, zAxis, v;
AD_Vect3D vx, vy, vz, ptmp;
AD_Quaternion q1, q2;
AD_Matrix rot1, rot2, rot12, pretrans;
if (!p_CurrentCamera) return;
// estrazione assi locali
mat_get_row(&p_CurrentCamera->p_CurrentRotationMatrix, 0, &xAxis);
mat_get_row(&p_CurrentCamera->p_CurrentRotationMatrix, 1, &yAxis);
mat_get_row(&p_CurrentCamera->p_CurrentRotationMatrix, 2, &zAxis);
// creazioni delle matrici di rotazione
quat_set(&q1, xAxis.x, xAxis.y, xAxis.z, ax);
quat_set(&q2, yAxis.x, yAxis.y, yAxis.z, ay);
quat_quat_to_rotquat(&q1); quat_rotquat_to_matrix(&q1, &rot1);
quat_quat_to_rotquat(&q2); quat_rotquat_to_matrix(&q2, &rot2);
mat_mul(&rot1, &rot2, &rot12);
// ruoto la posizione
vect_sub(&p_CurrentCamera->p_CurrentPosition,
&p_CurrentCamera->p_CurrentTargetPosition,
&cameradir);
mat_mulvect(&rot12, &cameradir, &v);
vect_add(&v, &p_CurrentCamera->p_CurrentTargetPosition,
&p_CurrentCamera->p_CurrentPosition);
// ruoto il target
/*vect_sub(&p_CurrentCamera->p_CurrentTargetPosition,
&p_CurrentCamera->p_CurrentPosition,
&cameradir);
mat_mulvect(&rot12, &cameradir, &v);
vect_add(&v, &p_CurrentCamera->p_CurrentPosition,
&p_CurrentCamera->p_CurrentTargetPosition);*/
mat_mulvect(&rot12, &xAxis, &vx); vect_auto_normalize(&vx);
mat_mulvect(&rot12, &yAxis, &vy); vect_auto_normalize(&vy);
mat_mulvect(&rot12, &zAxis, &vz); vect_auto_normalize(&vz);
mat_insert_row(&p_CurrentCamera->p_CurrentRotationMatrix, 0, &vx);
mat_insert_row(&p_CurrentCamera->p_CurrentRotationMatrix, 1, &vy);
mat_insert_row(&p_CurrentCamera->p_CurrentRotationMatrix, 2, &vz);
vect_neg(&p_CurrentCamera->p_CurrentPosition, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
mat_mul(&p_CurrentCamera->p_CurrentRotationMatrix,
&pretrans,
&p_CurrentCamera->p_WorldMatrix);
}
void AD_Object3D::get_vertex_normal (int16 quale, AD_Vect3D *vnorm)
{
int16 tt;
AD_Vect3D somma;
AD_Vertex3D *quale_ptr;
vect_set(&somma, 0, 0, 0);
quale_ptr=&vertex3D[quale];
for (tt=0; tt<num_tria; tt++)
{
if ((tria[tt].v1==quale_ptr) || (tria[tt].v2==quale_ptr) || (tria[tt].v3==quale_ptr))
vect_add(&somma, &tria[tt].normal, &somma);
}
vect_normalize(&somma);
vect_copy(&somma, vnorm);
}
void AD_Object3D::precalc_radius(void)
{
int16 i;
float4 aux=-1.0, mn;
AD_Vect3D midp, paux;
vect_set(&midp, 0, 0, 0);
for (i=0; i<num_vertex3D; i++)
{
vect_add(&midp, &vertex3D[i].point, &midp);
}
vect_scale(&midp, 1.0f/(float)num_vertex3D, &midp);
vect_copy(&midp, &mid_point);
for (i=0; i<num_vertex3D; i++)
{
vect_sub(&vertex3D[i].point, &midp, &paux);
mn=vect_lenght(&paux);
if (mn>aux) aux=mn;
}
radius=aux;
}
double find_cylinder_intersection(t_cylinder *cy, t_ray *r)
{
t_vect *abc;
double d;
double rslt;
t_vect *point;
abc = coeff_cylinder(cy, r);
d = pow(abc->y, 2) - abc->x * abc->z;
if (d > 0)
{
rslt = ((-abc->y - sqrt(d)) / abc->x) - 0.000001 > 0 ?
(-abc->y - sqrt(d)) / abc->x - 0.000001 :
(-abc->y + sqrt(d)) / abc->x - 0.000001;
point = vect_add(r->origin, vect_mult(r->direction, rslt));
if (!check_finite_cyl(cy, point))
rslt = -1;
}
else
rslt = -1;
delete_vect(abc);
return (rslt);
}
/*--- calc_du2: calculates the first part of the derivative dUdQ ---*/
void calc_dU2(CHAIN * C, double du2[][3])
{
double Rij[3], mn, mm, temp[3], a;
for(int i=0; i<C->N-1; i++){
du2[i][0] = 0; du2[i][1] = 0; du2[i][2] = 0; //calculated derivative dUdQ piece two, see 2Mar11 BEMRI notes
for(int m = 2; m < C->N; m++){
for(int n = 0; n < m-1; n++){
if(i >= n && i <= m-1){
get_Rij(C,n,m,Rij); //find new Rij vector
mn = C->mass[C->chain[n]];
mm = C->mass[C->chain[m]];
a = -G*mn*mm/( pow(mag(Rij,3),3) );
for(int l = n; l < m; l++)
{
vect_mult_scalar(C->R[l],a,temp,3);
vect_add(du2[i],temp,du2[i],3);
}
}
}}}
return;
}
t_inter *find_cylinders_intersection(t_ray *ray)
{
double mininter;
double inter;
t_vect *normal;
t_color *color;
t_cylinder *c;
mininter = -1;
c = get_scene()->cylinders;
while (c != NULL)
{
inter = find_cylinder_intersection(c, ray);
if (inter > ACCURACY && (inter < mininter || mininter == -1))
{
mininter = inter;
normal = get_normal_at_cylinder(c, vect_add(ray->origin,
vect_mult(ray->direction, inter)));
color = c->color;
}
c = c->next;
}
return (new_inter(normal, mininter, color));
}
/** Compute the center position from several radars sensors, return 1 if
* any. */
static uint8_t
radar_hit_center (uint8_t valid[], vect_t hit[], uint8_t sensor_nb,
vect_t *obs_pos)
{
uint8_t i, hit_nb = 0;
vect_t hit_center = { 0, 0 };
for (i = 0; i < sensor_nb; i++)
{
if (valid[i])
{
vect_add (&hit_center, &hit[i]);
hit_nb++;
}
}
if (hit_nb > 1)
vect_scale_f824 (&hit_center, 0x1000000l / hit_nb);
if (hit_nb)
{
*obs_pos = hit_center;
return 1;
}
else
return 0;
}
void
load_level(Level *level, char *xpm[], Rect_Vect *overlords, RGB_Color color, size_t screen_width) {
size_t width, height, colors, chars_on_px;
sscanf(xpm[0], "%zu %zu %zu %zu", &width, &height, &colors, &chars_on_px);
if (width != screen_width) {
error("load_level: map width(%d) should be the same as screen_width(%d)\n", width, screen_width);
} else if (chars_on_px != 1) {
error("load_level: only one char on pixel allowed in map format\n");
}
char land_ch = '\0';
char overlord_ch = '\0';
char movelord_ch = '\0';
for (int i = 1; i <= colors; i++) {
//I assume here that colors are in format: #XXXXXX
char ch, code[8];
sscanf(xpm[i], "%1c %*c %s", &ch, code);
if (strcmp(code, "#000000") == 0) {
land_ch = ch;
} else if (strcmp(code, "#FF0000") == 0){
overlord_ch = ch;
} else if (strcmp(code, "#FFFF00") == 0){
movelord_ch = ch;
}
}
if (land_ch == '\0') {
error("load_level: cannot find LEVEL_RIEVR_MASK in mapfile(%d, %d, %d)", color.r, color.g, color.b);
}
level->boxes.tab = NULL;
overlords->tab = NULL;
overlords->size = 0;
size_t lv_start_line = colors + 1;
int movelord_start = -1;
for (size_t i = lv_start_line; i < height + lv_start_line; i++) {
size_t w = 0, start_x = 0;
for (size_t j = 0; j < strlen(xpm[i]); j++) {
Rect rect;
if (xpm[i][j] == land_ch) {
w++;
} else {
if (w > 0) {
rect.w = w;
rect.h = 1;
rect.x = start_x;
rect.y = i - lv_start_line;
vect_add(rect, &level->boxes);
w = 0;
start_x = j;
}
start_x++;
if (xpm[i][j] == overlord_ch && !part_of_rect(overlord_ch, xpm, i, j)) {
Rect overlord;
overlord.x = j + 5;
overlord.y = i - lv_start_line + 5;
overlord.w = 38;
overlord.h = 38;
overlord.from = overlord.to = 0;
vect_add(overlord, overlords);
printf("overlord: %zu, %zu\n", overlord.x, overlord.y);
} else if (xpm[i][j] == movelord_ch) {
int part = part_of_rect(movelord_ch, xpm, i, j);
if (part == 0) {
movelord_start = j;
//end of the first line of rentangle
} else if (part == 1) {
Rect overlord;
overlord.x = movelord_start + 5;
overlord.y = i - lv_start_line + 5;
overlord.w = 38;
overlord.h = 38;
overlord.from = overlord.x;
overlord.to = j - overlord.w;
vect_add(overlord, overlords);
printf("movelord: x: %zu, y: %zu start: %d, end: %d\n", overlord.x, overlord.y, overlord.from, overlord.to);
}
}
}
}
if (w != 0) {
Rect rect;
rect.w = w;
rect.h = 1;
rect.x = start_x;
rect.y = i - lv_start_line;
vect_add(rect, &level->boxes);
}
}
}
void server_thread_generate_moves( server_thread_t* svt, int cycle )
{
int i, j;
tm_sv_client_t* cl;
//change quest_times to have quests on/off
int is_quest = ( sv.wl_quest_count &&
cycle >= sv.quest_times[sv.quest_id] &&
cycle < sv.quest_times[sv.quest_id] + sv.wl_quest_duration
);
for( i = 0; i < sv.n_clients; i++ )
{
cl = sv.clients[i];
if( cl->tid != svt->tid ) continue;
tm_entity_movable_t* pl = cl->player;
rect_t pl_r;
rect_init4( &pl_r, pl->r.v1.x, pl->r.v1.y, pl->r.v2.x, pl->r.v2.y );
value_t first_pl_dir = -1;
for( j = 0; j < sv.n_multiple_actions; j++ )
{
if( !sv.randomized_actions ) cl->m_actions[j][M_ACT_ID] = sv.m_actions[j];
else
{
int k, aux = rand_n( 100 );
for( k = 0; k < n_actions; k++ )
if( aux < sv.m_actions_ratios[k] ) break;
assert( k < n_actions );
cl->m_actions[j][M_ACT_ID] = k;
}
if( cl->m_actions[j][M_ACT_ID] == AC_MOVE )
{
//burceam: so, if I understand this correctly, we give random direction and speed
//to players; then a bit below, (if( is_quest && ( (act_index % 2) != 0 ) )
//we generate a quest move, which will supersede the randomly-establised direction.
cl->m_actions[j][M_ACT_SPD] = attribute_type_rand( &entity_types[ET_PLAYER]->attr_types[PL_SPEED] );
cl->m_actions[j][M_ACT_DIR] = attribute_type_rand( &entity_types[ET_PLAYER]->attr_types[PL_DIR] );
int act_index = cycle * sv.n_multiple_actions + j;
if( is_quest && ( (act_index % 2) == 0 ) )
{
cl->m_actions[j][M_ACT_SPD] = cl->m_actions[j][M_ACT_SPD] / 4;
if( entity_types[ET_PLAYER]->attr_types[PL_SPEED].min > cl->m_actions[j][M_ACT_SPD] )
cl->m_actions[j][M_ACT_SPD] = entity_types[ET_PLAYER]->attr_types[PL_SPEED].min;
}
if( is_quest && ( (act_index % 2) != 0 ) )
cl->m_actions[j][M_ACT_DIR] = server_thread_generate_quest_move( &pl_r, pl->ent_id );
if( sv.straight_move )
{
if( first_pl_dir == -1 )
first_pl_dir = cl->m_actions[j][M_ACT_DIR];
else
cl->m_actions[j][M_ACT_DIR] = first_pl_dir;
}
vect_t move_v;
vect_scale( &dirs[cl->m_actions[j][M_ACT_DIR]], cl->m_actions[j][M_ACT_SPD], &move_v );
vect_add( &pl_r.v1, &move_v, &pl_r.v1 );
vect_add( &pl_r.v2, &move_v, &pl_r.v2 );
// TO DO: change to allow multiple move records in file
//server_register_player_move( pl, i );
#ifdef PRINT_MOVES
printf( "cl:%3d cycle:%3d j: %d ; ", cl->cl_id, cycle, j );
printf( "pos: %3d,%3d - last_dist %d ; ", (coord_t)pl->r.v1.x, (coord_t)pl->r.v1.y, cl->last_dist );
printf( "act: %5s spd: %d dir: %5s\n", action_names[ cl->m_actions[j][M_ACT_ID] ],
cl->m_actions[j][M_ACT_SPD], dir_names[ cl->m_actions[j][M_ACT_DIR] ] );
#endif
}
}
}
}
color* trace( ray *aray, primitive *scene, int depth, float refr, float *dist,
int shadows ){
intersection *isect;
primitive *prim, *iter;
vector isect_pt, pn, lv, ln, tmpv1, tmpv2;
ray tmpr;
float tmpf1, tmpf2, tmpf3, shade;
color *tmpc;
color *c = (color*)malloc( sizeof( color ) );
if( c == NULL ) {
fprintf( stderr, "*** error: could not allocate color memory\n" );
exit( 1 );
}
c->x = 0.0f;
c->y = 0.0f;
c->z = 0.0f;
isect = intersect( aray, scene );
if( isect == NULL ) {
return c;
}
*dist = isect->dist;
prim = isect->prim;
if( prim->is_light ) {
vect_copy( c, &(isect->prim->mat.col) );
free( isect );
return c;
}
vect_copy( &isect_pt, aray->dir );
vect_multf( &isect_pt, isect->dist );
vect_add( &isect_pt, aray->origin );
prim->normal( prim, &isect_pt, &pn );
iter = scene;
while( iter != NULL ) {
if( iter->is_light ) {
vect_copy( &lv, &iter->center );
vect_sub( &lv, &isect_pt );
vect_copy( &ln, &lv );
vect_normalize( &ln );
shade = calc_shade( iter, &isect_pt, &lv, &ln, scene, shadows );
if( shade > 0.0f ) {
/* determine the diffuse component */
tmpf1 = prim->mat.diffuse;
if( tmpf1 > 0.0f ) {
tmpf2 = vect_dot( &pn, &ln );
if( tmpf2 > 0.0f ) {
tmpf1 *= tmpf2 * shade;
vect_copy( &tmpv1, &prim->mat.col );
vect_mult( &tmpv1, &iter->mat.col );
vect_multf( &tmpv1, tmpf1 );
vect_add( c, &tmpv1 );
}
}
/* determine the specular component */
tmpf1 = prim->mat.specular;
if( tmpf1 > 0.0f ) {
vect_copy( &tmpv1, &pn );
vect_copy( &tmpv2, &ln );
tmpf2 = 2.0f * vect_dot( &ln, &pn );
vect_multf( &tmpv1, tmpf2 );
vect_sub( &tmpv2, &tmpv1 );
tmpf2 = vect_dot( aray->dir, &tmpv2 );
if( tmpf2 > 0.0f ) {
tmpf1 = powf( tmpf2, 20.0f ) * tmpf1 * shade;
vect_copy( &tmpv1, &iter->mat.col );
vect_multf( &tmpv1, tmpf1 );
vect_add( c, &tmpv1 );
}
}
}
}
iter = iter->next;
}
/* calculate reflection */
if( prim->mat.refl > 0.0f && depth < TRACE_DEPTH ) {
vect_copy( &tmpv1, &pn );
vect_multf( &tmpv1, 2.0f * vect_dot( &pn, aray->dir ) );
vect_copy( &tmpv2, aray->dir );
vect_sub( &tmpv2, &tmpv1 );
vect_copy( &tmpv1, &tmpv2 );
vect_multf( &tmpv1, EPSILON );
vect_add( &tmpv1, &isect_pt );
tmpr.origin = &tmpv1;
tmpr.dir = &tmpv2;
tmpc = trace( &tmpr, scene, depth + 1, refr, &tmpf1, shadows );
vect_multf( tmpc, prim->mat.refl );
vect_copy( &tmpv1, &prim->mat.col );
vect_mult( &tmpv1, tmpc );
vect_add( c, &tmpv1 );
free( tmpc );
}
/* calculate refraction */
if( prim->mat.is_refr && depth < TRACE_DEPTH ) {
vect_copy( &tmpv1, &pn );
if( isect->inside ) {
vect_multf( &tmpv1, -1.0f );
}
tmpf1 = refr / prim->mat.refr;
tmpf2 = -( vect_dot( &tmpv1, aray->dir ) );
tmpf3 = 1.0f - tmpf1 * tmpf1 * (1.0f - tmpf2 * tmpf2);
if( tmpf3 > 0.0f ) {
vect_copy( &tmpv2, aray->dir );
vect_multf( &tmpv2, tmpf1 );
vect_multf( &tmpv1, tmpf1 * tmpf2 - sqrtf( tmpf3 ) );
vect_add( &tmpv1, &tmpv2 );
vect_copy( &tmpv2, &tmpv1 );
vect_multf( &tmpv2, EPSILON );
vect_add( &tmpv2, &isect_pt );
tmpr.origin = &tmpv2;
tmpr.dir = &tmpv1;
tmpc = trace( &tmpr, scene, depth + 1, refr, &tmpf1, shadows );
vect_copy( &tmpv1, &prim->mat.col );
vect_multf( &tmpv1, prim->mat.absorb * tmpf1 );
tmpv2.x = expf( -tmpv1.x );
tmpv2.y = expf( -tmpv1.y );
tmpv2.z = expf( -tmpv1.z );
vect_mult( tmpc, &tmpv2 );
vect_add( c, tmpc );
free( tmpc );
}
}
free( isect );
if( c->x > 1.0f ) {
c->x = 1.0f;
}
else if( c->x < 0.0f ) {
c->x = 0.0f;
}
if( c->y > 1.0f ) {
c->y = 1.0f;
}
else if( c->y < 0.0f ) {
c->y = 0.0f;
}
if( c->z > 1.0f ) {
c->z = 1.0f;
}
else if( c->z < 0.0f ) {
c->z = 0.0f;
}
return c;
}
void renderSingleFrame()
{
coord_t px,py; /* player position */
value_t life;
float pfx,pfy; /* interpolated player position (for main player) */
if( !cl.updated ) return;
SDL_LockMutex(cl.game_mutex);
px = cl.pl->r.v1.x;
py = cl.pl->r.v1.y;
life = cl.pl->attrs[PL_LIFE];
/* get new player position */
updatePosition(&pmd, px,py);
pfx = pmd.pfx;
pfy = pmd.pfy;
/* compute render range */
vect_init( &cl.pl->r.v1, (coord_t)pfx, (coord_t)pfy );
vect_add( &cl.pl->r.v1, &cl.pl->size, &cl.pl->r.v2 );
rect_t render_r = game_action_range( AC_VIEW, cl.pl, &wm.map_r );
vect_init( &cl.pl->r.v1, px, py );
vect_add( &cl.pl->r.v1, &cl.pl->size, &cl.pl->r.v2 );
/* begin rendering */
w.beginRender();
/* set camera and lights */ /* position */ /* look at */ /* up direction */
action_range_t* ar = &action_ranges[AC_VIEW];
gluLookAt( pfx-ar->front,1.5*ar->front,pfy-ar->front, pfx-0.1*ar->front,0.0,pfy-0.1*ar->front, 0,1,0);
/* draw floor */
int i, j;
for ( i = render_r.v1.x; i <= render_r.v2.x; i++ )
for ( j = render_r.v1.y; j <= render_r.v2.y; j++ )
floor_model->render(i,j);
/* draw entities */
int et;
for( et = n_entity_types-1; et >= 0; et-- )
{
pentity_set_t* pe_set = worldmap_get_entities( &render_r, 1 << et );
elem_t* pos;
pentity_set_for_each( pos, pe_set )
render_entity( ((pentity_t*)pos)->ent );
pentity_set_destroy( pe_set );
}
SDL_UnlockMutex(cl.game_mutex);
/* get coordinates for the status area */
SDL_Surface *screen = w.getScreen();
SDL_Rect rect;
rect.w = cl.resx;
rect.h = 40;
rect.x = 0;
rect.y = cl.resy - rect.h;
/* display status */
SDL_FillRect( screen, &rect, SDL_MapRGB( screen->format, 0x00, 0x00, 0x00 ) );
SDL_UpdateRect(screen, rect.x,rect.y, rect.w,rect.h);
char sbuffer[256];
SDL_WM_SetCaption( cl.name, NULL);
if ( cl.state != PLAYING ) sprintf(sbuffer, "%s", client_states[cl.state]);
else sprintf(sbuffer, "playing -> %s ( coord = %d,%d life = %d )", AI_state_names[cl.purpose], px, py, life);
font->render(rect.x + 9, rect.y + 2, sbuffer, screen);
sprintf(sbuffer, "update interval: average = %.1lfms, last = %.1lfms, fps = %.1f",
cl.average_update_interval, cl.last_update_interval, 1000.0 / frame_render_interval );
font->render(rect.x + 9, rect.y + 21, sbuffer, screen);
SDL_UpdateRect(screen, rect.x,rect.y, rect.w,rect.h);
/* finish frame rendering */
updateMovementDataVectors();
w.endRender();
}
void AD_Object3D::paint_bones(float4 pos, AD_Camera *telecamera, AD_Omnilight *omnilight)
{
int w, wl, i;
AD_Vect3D p1, p2, vtmp, p3;
AD_Vect3D light_vertex_vector;
float invz, cosalpha, dist;
Skin_Bone *b;
AD_Matrix mat_obj_cam;
// si portano le luci nello spazio della telecamera
for (w=0; w<num_omni; w++)
{
mat_mulvect(&telecamera->currentmatrix,
&omnilight[w].currentpos,
&omnilight[w].currentpos_inobject);
}
if ((bones_matrix!=(AD_Matrix **)NULL) &&
(skin_modifier==(Skin_Bone **)NULL))
{
// calcolo matrice di trasformazione dell'oggetto
for (w=0; w<num_vertex3D; w++)
{
vertex3D[w].flags=counter_for_resetting_vertex;
mat_mulvect(bones_matrix[w], &vertex3D[w].point, &p1);
mat_mulvect(&telecamera->currentmatrix, &p1, &vertex3D[w].tpoint);
if (vertex3D[w].tpoint.z > znear)
{
invz=1.0f/vertex3D[w].tpoint.z;
vertex2D[w].xs=telecamera->prospettivaX*(vertex3D[w].tpoint.x*invz) + screen_Xadd;
vertex2D[w].ys=screen_Yadd - telecamera->prospettivaY*(vertex3D[w].tpoint.y*invz);
vertex2D[w].z=(vertex3D[w].tpoint.z-znear)*inv_zfar_znear;
vertex2D[w].dist=invz;
}
else vertex3D[w].flags|=1;
vtmp.x=vtmp.y=vtmp.z=5;
for (wl=0; wl<num_omni; wl++)
{
vect_sub_inline(&omnilight[wl].currentpos, &p1, &light_vertex_vector);
mat_mulvectenv(bones_matrix_rot[w], &normals[w], &p2);
cosalpha=vect_dot(&p2, &light_vertex_vector);
// cosalpha=vect_dot(&normals[w], &light_vertex_vector);
if (is_float_positive(cosalpha))
{
dist=1.0f/vect_fast_lenght(&light_vertex_vector);
cosalpha*=dist;
vtmp.x+=cosalpha*omnilight[wl].currentcolor.x;
vtmp.y+=cosalpha*omnilight[wl].currentcolor.y;
vtmp.z+=cosalpha*omnilight[wl].currentcolor.z;
}
}
if (FP_BITS(vtmp.x)>FP_BITS(RGB_MAXVALUE)) FP_BITS(vertex2D[w].R)=FP_BITS(RGB_MAXVALUE);
else FP_BITS(vertex2D[w].R)=FP_BITS(vtmp.x);
if (FP_BITS(vtmp.y)>FP_BITS(RGB_MAXVALUE)) FP_BITS(vertex2D[w].G)=FP_BITS(RGB_MAXVALUE);
else FP_BITS(vertex2D[w].G)=FP_BITS(vtmp.y);
if (FP_BITS(vtmp.z)>FP_BITS(RGB_MAXVALUE)) FP_BITS(vertex2D[w].B)=FP_BITS(RGB_MAXVALUE);
else FP_BITS(vertex2D[w].B)=FP_BITS(vtmp.z);
}
for (w=0; w<num_tria; w++)
{
if (((tria[w].v1->flags + tria[w].v2->flags + tria[w].v3->flags + 1) & 3) !=0 )
{
if (tria[w].materiale->flags & IS_TRASPARENT)
*list_to_paint_trasparent++=&tria[w];
else *tria[w].materiale->my_tria_list++=&tria[w];
}
}
for (w=0; w<num_tria_RGB; w++)
{
if (((tria_RGB[w].v1->flags + tria_RGB[w].v2->flags + tria_RGB[w].v3->flags + 1) & 3) !=0 )
{
if (tria_RGB[w].materiale->flags & IS_TRASPARENT)
*list_to_paint_trasparent++=&tria_RGB[w];
else *tria_RGB[w].materiale->my_tria_list++=&tria_RGB[w];
}
}
}
else
if ((bones_matrix==(AD_Matrix **)NULL) &&
(skin_modifier!=(Skin_Bone **)NULL))
{
mat_mul(&telecamera->currentmatrix, ¤tmatrix, &mat_obj_cam);
for (w=0; w<num_vertex3D; w++)
{
vertex3D[w].flags=counter_for_resetting_vertex;
b=skin_modifier[w];
i=0;
vect_set(&p3, 0, 0, 0);
mat_mulvect(¤tmatrix, &vertex3D[w].point, &p1);
while (b[i].skin_matrix!=(AD_Matrix *)NULL)
{
mat_mulvect(b[i].skin_matrix, &p1, &p2);
p2.x*=b[i].weight;
p2.y*=b[i].weight;
p2.z*=b[i].weight;
vect_add(&p3, &p2, &p3);
i++;
}
mat_mulvect(&telecamera->currentmatrix, &p3, &vertex3D[w].tpoint);
if (vertex3D[w].tpoint.z > znear)
{
invz=1.0f/vertex3D[w].tpoint.z;
vertex2D[w].xs=telecamera->prospettivaX*(vertex3D[w].tpoint.x*invz) + screen_Xadd;
vertex2D[w].ys=screen_Yadd - telecamera->prospettivaY*(vertex3D[w].tpoint.y*invz);
vertex2D[w].z=(vertex3D[w].tpoint.z-znear)*inv_zfar_znear;
vertex2D[w].dist=invz;
vertex2D[w].R=vertex2D[w].z*255;
vertex2D[w].G=vertex2D[w].z*255;
vertex2D[w].B=vertex2D[w].z*255;
}
else vertex3D[w].flags|=1;
}
for (w=0; w<num_tria; w++)
{
if (((tria[w].v1->flags + tria[w].v2->flags + tria[w].v3->flags + 1) & 3) !=0 )
{
if (tria[w].materiale->flags & IS_TRASPARENT)
*list_to_paint_trasparent++=&tria[w];
else *tria[w].materiale->my_tria_list++=&tria[w];
}
}
for (w=0; w<num_tria_RGB; w++)
{
if (((tria_RGB[w].v1->flags + tria_RGB[w].v2->flags + tria_RGB[w].v3->flags + 1) & 3) !=0 )
{
if (tria_RGB[w].materiale->flags & IS_TRASPARENT)
*list_to_paint_trasparent++=&tria_RGB[w];
else *tria_RGB[w].materiale->my_tria_list++=&tria_RGB[w];
}
}
}
}
void AD_PatchObject::Evaluate_vDerivate(AD_Patch *p, float pu, float pv, AD_Vect3D *r)
{
AD_Vect3D t;
AD_Vect3D *vp = verteces;
AD_Vect3D *vecp = vectors;
int *v=p->vert;
int *vec=p->vect;
int *interior=p->interior;
float pu2 = pu * pu;
float pu1 = 1.0f - pu;
float pu12 = pu1 * pu1;
float u0 = pu12 * pu1;
float u1 = 3.0f * pu * pu12;
float u2 = 3.0f * pu2 * pu1;
float u3 = pu2 * pu;
// calcolo la derivata in v delle funzioni base
float pv2 = pv * pv;
float pv1 = 1.0f - pv;
float v0 = -3.0f * pv1 * pv1;
float v1 = 3.0f * pv1 * (1.0f - 3*pv);
float v2 = 3.0f * pv * (2.0f - 3*pv);
float v3 = 3.0f * pv2;
vect_scale(&vp[v[0]], u0*v0, r);
vect_scale(&vecp[vec[7]], u1*v0, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[6]], u2*v0, &t);
vect_add(r, &t, r);
vect_scale(&vp[v[3]], u3*v0, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[0]], u0*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[0]], u1*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[3]], u2*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[5]], u3*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[1]], u0*v2, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[1]], u1*v2, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[2]], u2*v2, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[4]], u3*v2, &t);
vect_add(r, &t, r);
vect_scale(&vp[v[1]], u0*v3, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[2]], u1*v3, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[3]], u2*v3, &t);
vect_add(r, &t, r);
vect_scale(&vp[v[2]], u3*v3, &t);
vect_add(r, &t, r);
}
void AD_Object3D::init_normals(void)
// le normali dei triangoli devono essere gia' state calcolate
// vertici triangoli e smoothing groups devono già essare in memoria
{
#define normal_err 0.0001f
// indica l'errore massimo possibile x riciclare una normale: + e' alto il numero piu'
// si perde qualita' visiva e si guadagna velocita': bisogna trovare un buon compromesso
int *condivisi;
int *smooth, *nosmooth; // array di triangoli condivisi da smoothare
AD_Vect3D *tempnormal;
AD_Vect3D normadd;
int num_condivisi, i, j, tr, num_smooth, num_nosmooth;
int num_normal, norm;
float maxerr, err;
condivisi=new int[num_tria]; // nel caso peggiore tutti i triangoli sono condivisi
smooth=new int[num_tria];
nosmooth=new int[num_tria];
tempnormal=new AD_Vect3D[num_tria*3]; // nel caso peggiore ci sono 3 normali per triangolo
num_normal=0;
for (i=0; i<num_vertex3D; i++)
{
// trovo i triangoli che condividono il vertice i
num_condivisi=0;
for (j=0; j<num_tria; j++)
{
if ((tria[j].v1==&vertex3D[i]) ||
(tria[j].v2==&vertex3D[i]) ||
(tria[j].v3==&vertex3D[i]))
{
condivisi[num_condivisi]=j;
num_condivisi++;
}
}
while (num_condivisi>0)
{
tr=condivisi[0]; // triangolo di riferimento
num_smooth=0;
num_nosmooth=0;
smooth[num_smooth]=tr;
num_smooth++;
for(j=1; j<num_condivisi; j++)
{
if ((triasmoothgroup[tr] & triasmoothgroup[condivisi[j]])!=0)
{
smooth[num_smooth]=condivisi[j];
num_smooth++;
}
else
{
nosmooth[num_nosmooth]=condivisi[j];
num_nosmooth++;
}
}
// cacolo la normale
vect_set(&normadd, 0, 0, 0);
for (j=0; j<num_smooth; j++)
vect_add(&normadd, &tria[smooth[j]].normal, &normadd);
vect_normalize(&normadd);
// cerco se ne esiste gia' una molto simile
j=0;
norm=-1;
maxerr=normal_err;
while (j < num_normal)
{
// trovo l'errore massimo della normale j
err=fmax(fmax(fabsf(normadd.x-tempnormal[j].x),
fabsf(normadd.y-tempnormal[j].y)),
fabsf(normadd.z-tempnormal[j].z));
if (err < maxerr)
{
// trovata normale + precisa
maxerr=err;
norm=j;
}
j++;
}
if (norm==-1)
{ // non trovata: la creo
vect_copy(&normadd, &tempnormal[num_normal]);
norm=num_normal;
num_normal++;
}
for (j=0; j<num_smooth; j++)
{
// la assegno al triangolo (cercando il vertice giusto)
if (tria[smooth[j]].v1==&vertex3D[i]) tria[smooth[j]].n1=&tempnormal[norm];
if (tria[smooth[j]].v2==&vertex3D[i]) tria[smooth[j]].n2=&tempnormal[norm];
if (tria[smooth[j]].v3==&vertex3D[i]) tria[smooth[j]].n3=&tempnormal[norm];
}
// tolgo quelli assegnati e ricompatto gli altri
for (j=0; j<num_nosmooth; j++) condivisi[j]=nosmooth[j];
num_condivisi-=num_smooth;
}
}
// copio la parte usata di tempnormal in normal
normals = new AD_Vect3D[num_normal];
num_normals=num_normal;
for (j=0; j<num_normal; j++)
{
vect_copy(&tempnormal[j], &normals[j]);
for (i=0; i<num_tria; i++)
{
if (tria[i].n1==&tempnormal[j]) tria[i].n1=&normals[j];
if (tria[i].n2==&tempnormal[j]) tria[i].n2=&normals[j];
if (tria[i].n3==&tempnormal[j]) tria[i].n3=&normals[j];
}
}
delete [] condivisi;
delete [] smooth;
delete [] nosmooth;
delete [] tempnormal;
delete [] triasmoothgroup; // non servono piu'
}
void test_basic_vector_op(void)
{
/******************************/
/* Test the vector operations */
/******************************/
/*create two vectors and show them*/
int vector_length = 5;
double vector_a[5] = {2.5, -10.9, 15.8, 12.2, 7.9};
double vector_b[5] = {2.5, 0.2, 21.33, 70.1, -0.2};
double vector_c[5];
double result = 0.0;
/*seed the random generator*/
srand(time(NULL));
printf("The two vectors a and b\n");
show_vector(vector_a, vector_length);
show_vector(vector_b, vector_length);
/*vector addition*/
printf("c = a + b\n");
/*copy a*/
memcpy(vector_c, vector_a, vector_length*sizeof(double));
/*compute*/
vect_add(vector_c, vector_b, vector_length);
/*show expected and obtained results*/
printf("Expected: 5.0000 -10.7000 37.1300 82.3000 7.7000\n");
printf("Result: ");
show_vector(vector_c, vector_length);
/*vector subtraction*/
printf("c = a - b\n");
/*copy a*/
memcpy(vector_c, vector_a, vector_length*sizeof(double));
/*compute*/
vect_sub(vector_c, vector_b, vector_length);
/*show expected and obtained results*/
printf("Expected: 0.0000 -11.1000 -5.5300 -57.9000 8.1000\n");
printf("Result: ");
show_vector(vector_c, vector_length);
/*vector scalar multiplication*/
printf("c = a * 5\n");
/*copy a*/
memcpy(vector_c, vector_a, vector_length*sizeof(double));
/*compute*/
vect_scalar_multiply(vector_c, 5, vector_length);
/*show expected and obtained results*/
printf("Expected: 12.5000 -54.5000 79.0000 61.0000 39.5000\n");
printf("Result: ");
show_vector(vector_c, vector_length);
/*vector dot product*/
printf("c = a .* b\n");
/*compute*/
result = vect_dot_product(vector_a, vector_b, vector_length);
/*show expected and obtained results*/
printf("Expected: 1194.72\n");
printf("Result: %2.2f\n",result);
/*vector cross product*/
/*To be completed*/
/*vector norm*/
printf("c = |a|\n");
/*compute*/
result = vect_norm(vector_a, vector_length);
/*show expected and obtained results*/
printf("Expected: 24.2064\n");
printf("Result: %2.4f\n",result);
/*vector rand unit*/
printf("c is a random vector\n");
printf("|c| == 1\n");
/*compute*/
vect_rand_unit(vector_c, vector_length);
/*show expected and obtained results*/
printf("The vector: ");
show_vector(vector_c, vector_length);
result = vect_norm(vector_c, vector_length);
printf("Expected norm: 1.0000\n");
printf("Obtained norm: %2.4f\n",result);
}
/**
* Recursive Newton-Euler algorithm.
*
* @Note the parameter \p stride which is used to allow for input and output
* arrays which are 2-dimensional but in column-major (Matlab) order. We
* need to access rows from the arrays.
*
*/
void
newton_euler (
Robot *robot, /*!< robot object */
double *tau, /*!< returned joint torques */
double *qd, /*!< joint velocities */
double *qdd, /*!< joint accelerations */
double *fext, /*!< external force on manipulator tip */
int stride /*!< indexing stride for qd, qdd */
) {
Vect t1, t2, t3, t4;
Vect qdv, qddv;
Vect F, N;
Vect z0 = {0.0, 0.0, 1.0};
Vect zero = {0.0, 0.0, 0.0};
Vect f_tip = {0.0, 0.0, 0.0};
Vect n_tip = {0.0, 0.0, 0.0};
register int j;
double t;
Link *links = robot->links;
/*
* angular rate and acceleration vectors only have finite
* z-axis component
*/
qdv = qddv = zero;
/* setup external force/moment vectors */
if (fext) {
f_tip.x = fext[0];
f_tip.y = fext[1];
f_tip.z = fext[2];
n_tip.x = fext[3];
n_tip.y = fext[4];
n_tip.z = fext[5];
}
/******************************************************************************
* forward recursion --the kinematics
******************************************************************************/
if (robot->dhtype == MODIFIED) {
/*
* MODIFIED D&H CONVENTIONS
*/
for (j = 0; j < robot->njoints; j++) {
/* create angular vector from scalar input */
qdv.z = qd[j*stride];
qddv.z = qdd[j*stride];
switch (links[j].sigma) {
case REVOLUTE:
/*
* calculate angular velocity of link j
*/
if (j == 0)
*OMEGA(j) = qdv;
else {
rot_trans_vect_mult (&t1, ROT(j), OMEGA(j-1));
vect_add (OMEGA(j), &t1, &qdv);
}
/*
* calculate angular acceleration of link j
*/
if (j == 0)
*OMEGADOT(j) = qddv;
else {
rot_trans_vect_mult (&t3, ROT(j), OMEGADOT(j-1));
vect_cross (&t2, &t1, &qdv);
vect_add (&t1, &t2, &t3);
vect_add (OMEGADOT(j), &t1, &qddv);
}
/*
* compute acc[j]
*/
if (j == 0) {
t1 = *robot->gravity;
} else {
vect_cross(&t1, OMEGA(j-1), PSTAR(j));
vect_cross(&t2, OMEGA(j-1), &t1);
vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, ACC(j-1));
}
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
break;
case PRISMATIC:
/*
* calculate omega[j]
*/
if (j == 0)
*(OMEGA(j)) = qdv;
else
rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));
/*
* calculate alpha[j]
*/
if (j == 0)
*(OMEGADOT(j)) = qddv;
else
rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));
/*
* compute acc[j]
*/
if (j == 0) {
*ACC(j) = *robot->gravity;
} else {
vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));
vect_cross(&t3, OMEGA(j-1), PSTAR(j));
vect_cross(&t2, OMEGA(j-1), &t3);
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, ACC(j-1));
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
rot_trans_vect_mult(&t2, ROT(j), OMEGA(j-1));
vect_cross(&t1, &t2, &qdv);
scal_mult(&t1, &t1, 2.0);
vect_add(ACC(j), ACC(j), &t1);
vect_add(ACC(j), ACC(j), &qddv);
}
break;
}
/*
* compute abar[j]
*/
vect_cross(&t1, OMEGADOT(j), R_COG(j));
vect_cross(&t2, OMEGA(j), R_COG(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC_COG(j), &t1, &t3);
vect_add(ACC_COG(j), ACC_COG(j), ACC(j));
#ifdef DEBUG
vect_print("w", OMEGA(j));
vect_print("wd", OMEGADOT(j));
vect_print("acc", ACC(j));
vect_print("abar", ACC_COG(j));
#endif
}
} else {
/*
* STANDARD D&H CONVENTIONS
*/
for (j = 0; j < robot->njoints; j++) {
/* create angular vector from scalar input */
qdv.z = qd[j*stride];
qddv.z = qdd[j*stride];
switch (links[j].sigma) {
case REVOLUTE:
/*
* calculate omega[j]
*/
if (j == 0)
t1 = qdv;
else
vect_add (&t1, OMEGA(j-1), &qdv);
rot_trans_vect_mult (OMEGA(j), ROT(j), &t1);
/*
* calculate alpha[j]
*/
if (j == 0)
t3 = qddv;
else {
vect_add (&t1, OMEGADOT(j-1), &qddv);
vect_cross (&t2, OMEGA(j-1), &qdv);
vect_add (&t3, &t1, &t2);
}
rot_trans_vect_mult (OMEGADOT(j), ROT(j), &t3);
/*
* compute acc[j]
*/
vect_cross(&t1, OMEGADOT(j), PSTAR(j));
vect_cross(&t2, OMEGA(j), PSTAR(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC(j), &t1, &t3);
if (j == 0) {
rot_trans_vect_mult(&t1, ROT(j), robot->gravity);
} else
rot_trans_vect_mult(&t1, ROT(j), ACC(j-1));
vect_add(ACC(j), ACC(j), &t1);
break;
case PRISMATIC:
/*
* calculate omega[j]
*/
if (j == 0)
*(OMEGA(j)) = zero;
else
rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));
/*
* calculate alpha[j]
*/
if (j == 0)
*(OMEGADOT(j)) = zero;
else
rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));
/*
* compute acc[j]
*/
if (j == 0) {
vect_add(&qddv, &qddv, robot->gravity);
rot_trans_vect_mult(ACC(j), ROT(j), &qddv);
} else {
vect_add(&t1, &qddv, ACC(j-1));
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
}
vect_cross(&t1, OMEGADOT(j), PSTAR(j));
vect_add(ACC(j), ACC(j), &t1);
rot_trans_vect_mult(&t1, ROT(j), &qdv);
vect_cross(&t2, OMEGA(j), &t1);
scal_mult(&t2, &t2, 2.0);
vect_add(ACC(j), ACC(j), &t2);
vect_cross(&t2, OMEGA(j), PSTAR(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC(j), ACC(j), &t3);
break;
}
/*
* compute abar[j]
*/
vect_cross(&t1, OMEGADOT(j), R_COG(j));
vect_cross(&t2, OMEGA(j), R_COG(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC_COG(j), &t1, &t3);
vect_add(ACC_COG(j), ACC_COG(j), ACC(j));
#ifdef DEBUG
vect_print("w", OMEGA(j));
vect_print("wd", OMEGADOT(j));
vect_print("acc", ACC(j));
vect_print("abar", ACC_COG(j));
#endif
}
}
/******************************************************************************
* backward recursion part --the kinetics
******************************************************************************/
if (robot->dhtype == MODIFIED) {
/*
* MODIFIED D&H CONVENTIONS
*/
for (j = robot->njoints - 1; j >= 0; j--) {
/*
* compute F[j]
*/
scal_mult (&F, ACC_COG(j), M(j));
/*
* compute f[j]
*/
if (j == (robot->njoints-1))
t1 = f_tip;
else
rot_vect_mult (&t1, ROT(j+1), f(j+1));
vect_add (f(j), &t1, &F);
/*
* compute N[j]
*/
mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
vect_cross(&t4, OMEGA(j), &t3);
vect_add(&N, &t2, &t4);
/*
* compute n[j]
*/
if (j == (robot->njoints-1))
t1 = n_tip;
else {
rot_vect_mult(&t1, ROT(j+1), n(j+1));
rot_vect_mult(&t4, ROT(j+1), f(j+1));
vect_cross(&t3, PSTAR(j+1), &t4);
vect_add(&t1, &t1, &t3);
}
vect_cross(&t2, R_COG(j), &F);
vect_add(&t1, &t1, &t2);
vect_add(n(j), &t1, &N);
#ifdef DEBUG
vect_print("f", f(j));
vect_print("n", n(j));
#endif
}
} else {
/*
* STANDARD D&H CONVENTIONS
*/
for (j = robot->njoints - 1; j >= 0; j--) {
/*
* compute f[j]
*/
scal_mult (&t4, ACC_COG(j), M(j));
if (j != (robot->njoints-1)) {
rot_vect_mult (&t1, ROT(j+1), f(j+1));
vect_add (f(j), &t4, &t1);
} else
vect_add (f(j), &t4, &f_tip);
/*
* compute n[j]
*/
/* cross(pstar+r,Fm(:,j)) */
vect_add(&t2, PSTAR(j), R_COG(j));
vect_cross(&t1, &t2, &t4);
if (j != (robot->njoints-1)) {
/* cross(R'*pstar,f) */
rot_trans_vect_mult(&t2, ROT(j+1), PSTAR(j));
vect_cross(&t3, &t2, f(j+1));
/* nn += R*(nn + cross(R'*pstar,f)) */
vect_add(&t3, &t3, n(j+1));
rot_vect_mult(&t2, ROT(j+1), &t3);
vect_add(&t1, &t1, &t2);
} else {
/* cross(R'*pstar,f) */
vect_cross(&t2, PSTAR(j), &f_tip);
/* nn += R*(nn + cross(R'*pstar,f)) */
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, &n_tip);
}
mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
vect_cross(&t4, OMEGA(j), &t3);
vect_add(&t2, &t2, &t4);
vect_add(n(j), &t1, &t2);
#ifdef DEBUG
vect_print("f", f(j));
vect_print("n", n(j));
#endif
}
}
/*
* Compute the torque total for each axis
*
*/
for (j=0; j < robot->njoints; j++) {
double t;
Link *l = &links[j];
if (robot->dhtype == MODIFIED)
t1 = z0;
else
rot_trans_vect_mult(&t1, ROT(j), &z0);
switch (l->sigma) {
case REVOLUTE:
t = vect_dot(n(j), &t1);
break;
case PRISMATIC:
t = vect_dot(f(j), &t1);
break;
}
/*
* add actuator dynamics and friction
*/
t += l->G * l->G * l->Jm * qdd[j*stride]; // inertia
t += l->G * l->G * l->B * qd[j*stride]; // viscous friction
t += fabs(l->G) * (
(qd[j*stride] > 0 ? l->Tc[0] : 0.0) + // Coulomb friction
(qd[j*stride] < 0 ? l->Tc[1] : 0.0)
);
tau[j*stride] = t;
}
}
// valuta la pezza in (pu, pv) e restituisce il risultato in r
void AD_PatchObject::Evaluate_Patch(AD_Patch *p, float pu, float pv, AD_Vect3D *r)
{
AD_Vect3D t;
AD_Vect3D *vp = verteces_tr;
AD_Vect3D *vecp = vectors_tr;
int *v=p->vert;
int *vec=p->vect;
int *interior=p->interior;
float pu2 = pu * pu;
float pu1 = 1.0f - pu;
float pu12 = pu1 * pu1;
float u0 = pu12 * pu1;
float u1 = 3.0f * pu * pu12;
float u2 = 3.0f * pu2 * pu1;
float u3 = pu2 * pu;
float pv2 = pv * pv;
float pv1 = 1.0f - pv;
float pv12 = pv1 * pv1;
float v0 = pv12 * pv1;
float v1 = 3.0f * pv * pv12;
float v2 = 3.0f * pv2 * pv1;
float v3 = pv2 * pv;
vect_scale(&vp[v[0]], u0*v0, r);
vect_scale(&vecp[vec[7]], u1*v0, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[6]], u2*v0, &t);
vect_add(r, &t, r);
vect_scale(&vp[v[3]], u3*v0, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[0]], u0*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[0]], u1*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[3]], u2*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[5]], u3*v1, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[1]], u0*v2, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[1]], u1*v2, &t);
vect_add(r, &t, r);
vect_scale(&vecp[interior[2]], u2*v2, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[4]], u3*v2, &t);
vect_add(r, &t, r);
vect_scale(&vp[v[1]], u0*v3, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[2]], u1*v3, &t);
vect_add(r, &t, r);
vect_scale(&vecp[vec[3]], u2*v3, &t);
vect_add(r, &t, r);
vect_scale(&vp[v[2]], u3*v3, &t);
vect_add(r, &t, r);
}
void render( int width, int height, color ***image, primitive *scene,
int aa_level, int shadows ) {
float world_left, world_right, world_top, world_bot;
float delta_x, delta_y;
float screen_x, screen_y;
float tmpf;
int x, y;
int aa_x, aa_y;
int aa_root;
vector origin, dir;
ray r;
color *tmpc;
world_left = -(4.0f * ((float)width / (float)height) );
world_right = -world_left;
world_top = 4.0f;
world_bot = -4.0f;
/* The amount to shift for each pixel */
delta_x = (world_right - world_left) / width;
delta_y = (world_bot - world_top) / height;
origin.x = 0.0f;
origin.y = 0.0f;
origin.z = -5.0f;
aa_root = (int)sqrt( aa_level );
screen_y = world_top;
for( y = 0; y < height; y++ ) {
screen_x = world_left;
for( x = 0; x < width; x++ ) {
for( aa_x = 0; aa_x < aa_root; aa_x++ ) {
for( aa_y = 0; aa_y < aa_root; aa_y++ ) {
dir.x = screen_x + delta_x * aa_x / (float)aa_root;
dir.y = screen_y + delta_y * aa_y / (float)aa_root;
dir.z = 0.0f;
vect_sub( &dir, &origin );
vect_normalize( &dir );
r.origin = &origin;
r.dir = &dir;
tmpc = trace( &r, scene, 0, 1.0f, &tmpf, shadows );
if( image[y][x] == NULL ) {
image[y][x] = tmpc;
}
else {
vect_add( image[y][x], tmpc );
free( tmpc );
}
}
}
vect_multf( image[y][x], 1.0f / aa_level );
screen_x += delta_x;
}
screen_y += delta_y;
}
}
int calc_angles(binary * b)
{
double x,y,z,vx,vy,vz,r,v,k,h[3],R[3],V[3],e_vec[3],vxh1[3],vxh2[3],R_temp[3],theta,edotr,ip;
double i,z_hat[3] = {0,0,1}, ea, lan_calc, aop_calc, n[3];
double beta;
int ret=0;
//First translate to mass 1 rest frame
x = b->x2[0] - b->x1[0];
y = b->x2[1] - b->x1[1];
z = b->x2[2] - b->x1[2];
vx = b->v2[0] - b->v1[0];
vy = b->v2[1] - b->v1[1];
vz = b->v2[2] - b->v1[2];
r = sqrt(x*x + y*y + z*z);
v = sqrt(vx*vx + vy*vy + vz*vz);
k = G*b->mass1 + G*b->mass2; //here k is the standard gravitational parameter
//put together vectors of r and v
R[0] = x;
R[1] = y;
R[2] = z;
V[0] = vx;
V[1] = vy;
V[2] = vz;
//calculate specific angular momentum vector h
cross_prod(R,V,h);
//calculate the eccentricity vector
cross_prod(V,h,vxh1);
vect_mult_scalar(vxh1, 1/k, vxh2, 3);
vect_mult_scalar(R,-1/r,R_temp,3);
vect_add(vxh2,R_temp,e_vec,3);
//find true anomaly from r
edotr = dot_prod(e_vec,R,3);
theta = acos(edotr / ( mag(e_vec,3) * mag(R,3) ) );
if(dot_prod(R,V,3) < 0)
{
theta = 2*PI - theta;
ret = 1;
}
b->true_anomaly = theta;
i = acos(h[2]/mag(h,3));
//calculate n, unit normal vector
vect_mult_scalar(h,1/mag(h,3),n,3);
lan_calc = atan2(n[0],-n[1]);
lan_calc = fix_angle(lan_calc); //keep angles in range 0 to 2*pi
if(i==0) //zero inclination makes lan meaningless. Keep it at zero
lan_calc = 0;
beta = atan2(-cos(i) * sin(lan_calc) * x + cos(i) * cos(lan_calc) * y + sin(i) * z , cos(lan_calc) * x + sin(lan_calc) * y);
beta = fix_angle(beta); //keep angles in range 0 to 2*pi
aop_calc = PI + beta - theta;
aop_calc = fix_angle(aop_calc); //keep angles in range 0 to 2*pi
ea = acos((mag(e_vec,3) + cos(theta)) / (1.0 + mag(e_vec,3)*cos(theta)));
b->i_a = i;
b->la_n = lan_calc;
b->ao_p = aop_calc;
return ret;
}
float calc_shade( primitive *light, vector *isect_pt, vector *lv, vector *ln,
primitive *scene, int shadows ) {
float shade = 1.0f;
float l_dist;
int i;
ray r;
vector o, dir;
intersection *isect;
primitive *iter;
if( light->is_light == AREA_LIGHT && shadows > 1 && light->grid != NULL ) {
for( i = 0; i < shadows; i++ ) {
dir.x = light->grid[(i&63)*3] +
((float)rand()/RAND_MAX)*light->dx;
dir.y = light->grid[(i&63)*3+1] +
((float)rand()/RAND_MAX)*light->dy;
dir.z = light->grid[(i&63)*3+2] +
((float)rand()/RAND_MAX)*light->dz;
vect_sub( &dir, isect_pt );
l_dist = vect_length( &dir );
vect_multf( &dir, 1.0f / l_dist );
vect_copy( &o, &dir );
vect_multf( &o, EPSILON );
vect_add( &o, isect_pt );
r.origin = &o;
r.dir = &dir;
for( iter = scene; iter != NULL; iter = iter->next ) {
isect = iter->intersect( iter, &r );
if( isect != NULL &&
!iter->is_light && isect->dist < l_dist ) {
shade -= 1.0f / shadows;
free( isect );
break;
}
free( isect );
}
}
}
else {
/* setup the ray */
vect_copy( &o, ln );
vect_multf( &o, EPSILON );
vect_add( &o, isect_pt );
r.origin = &o;
r.dir = ln;
l_dist = vect_length( lv );
for( iter = scene; iter != NULL; iter = iter->next ) {
isect = iter->intersect( iter, &r );
if( isect != NULL && !iter->is_light && isect->dist < l_dist ) {
shade = 0.0f;
free( isect );
break;
}
free( isect );
}
}
return shade;
}
