static void calculate_reflected(t_ray *a, t_ray *b)
{
t_vector n;
t_point intersection;
intersection.x = a->origin.x + a->direction.x * a->inter_t;
intersection.y = a->origin.y + a->direction.y * a->inter_t;
intersection.z = a->origin.z + a->direction.z * a->inter_t;
if (a->closest->type == OBJ_SPHERE)
{
n.x = intersection.x - a->closest->origin.x;
n.y = intersection.y - a->closest->origin.y;
n.z = intersection.z - a->closest->origin.z;
}
else if (a->closest->type == OBJ_CYLINDER)
get_cylinder_normal(&n, a->closest, &intersection);
else if (a->closest->type == OBJ_CONE)
get_cone_normal(&n, a->closest, &intersection);
else
n = a->closest->normal;
b->origin = intersection;
vect_scale(&a->direction, -1);
b->direction.x = 2 * (vect_dot(&n, &a->direction)) * n.x - a->direction.x;
b->direction.y = 2 * (vect_dot(&n, &a->direction)) * n.y - a->direction.y;
b->direction.z = 2 * (vect_dot(&n, &a->direction)) * n.z - a->direction.z;
normalize_vector(&b->direction);
}
/**
* @brief Computes an estimation of ammo flying time
*
* @param w the weapon that shoot
* @param parent Parent of the weapon
* @param target Target of the weapon
*/
double pilot_weapFlyTime( Outfit *o, Pilot *parent, Vector2d *pos, Vector2d *vel)
{
Vector2d approach_vector, relative_location, orthoradial_vector;
double speed, radial_speed, orthoradial_speed, dist, t;
dist = vect_dist( &parent->solid->pos, pos );
/* Beam weapons */
if (outfit_isBeam(o))
{
if (dist > o->u.bem.range)
return INFINITY;
return 0.;
}
/* A bay doesn't have range issues */
if (outfit_isFighterBay(o))
return 0.;
/* Rockets use absolute velocity while bolt use relative vel */
if (outfit_isLauncher(o))
vect_cset( &approach_vector, - vel->x, - vel->y );
else
vect_cset( &approach_vector, VX(parent->solid->vel) - vel->x,
VY(parent->solid->vel) - vel->y );
speed = outfit_speed(o);
/* Get the vector : shooter -> target */
vect_cset( &relative_location, pos->x - VX(parent->solid->pos),
pos->y - VY(parent->solid->pos) );
/* Get the orthogonal vector */
vect_cset(&orthoradial_vector, VY(parent->solid->pos) - pos->y,
pos->x - VX(parent->solid->pos) );
radial_speed = vect_dot( &approach_vector, &relative_location );
radial_speed = radial_speed / VMOD(relative_location);
orthoradial_speed = vect_dot(&approach_vector, &orthoradial_vector);
orthoradial_speed = orthoradial_speed / VMOD(relative_location);
if( ((speed*speed - VMOD(approach_vector)*VMOD(approach_vector)) != 0) && (speed*speed - orthoradial_speed*orthoradial_speed) > 0)
t = dist * (sqrt( speed*speed - orthoradial_speed*orthoradial_speed ) - radial_speed) /
(speed*speed - VMOD(approach_vector)*VMOD(approach_vector));
else
return INFINITY;
/* if t < 0, try the other solution */
if (t < 0)
t = - dist * (sqrt( speed*speed - orthoradial_speed*orthoradial_speed ) + radial_speed) /
(speed*speed - VMOD(approach_vector)*VMOD(approach_vector));
/* if t still < 0, no solution */
if (t < 0)
return INFINITY;
return t;
}
/**
* @brief Determines the magnitude of the source vector components.
*
* @param[out] u Parallel component to reference vector.
* @param[out] v Perpendicular component to reference vector.
* @param source Source vector.
* @param reference_vector Reference vector.
*/
void vect_uv( double* u, double* v, Vector2d* source, Vector2d* reference_vector )
{
Vector2d unit_parallel, unit_perpendicular;
vect_uv_decomp(&unit_parallel, &unit_perpendicular, reference_vector);
*u = vect_dot(source, &unit_parallel);
*v = vect_dot(source, &unit_perpendicular);
}
void CGravityModifier::m_Force (float4 framepos, AD_Vect3D *pos, AD_Vect3D *vel, AD_Vect3D *accel)
{
AD_Vect3D posrel, force;
float4 dist;
if (p_Mapping==FORCE_PLANAR)
{
if (p_Decay!=0)
{
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(&p_CurrentPosition, pos, &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);
}
}
void AD_GravityModifier::Force (float framepos, AD_Vect3D *pos, AD_Vect3D *vel, AD_Vect3D *accel)
{
AD_Vect3D posrel;
float dist, stren;
stren=strenght;
if (mapping==FORCE_PLANAR)
{
if (decay!=0.0f)
{
vect_sub_inline(pos, ¤tpos, &posrel);
dist = vect_dot(&forza, &posrel);
stren *= (float)exp(-decay*dist);
}
accel->x=forza.x;
accel->y=forza.y;
accel->z=forza.z;
}
else
{
vect_sub_inline(¤tpos, pos, &forza);
dist = vect_fast_lenght(&forza);
if (dist!=0.0f) vect_scale_inline(&forza, 1.0f/dist, &forza);
if (decay!=0.0f)
stren *= (float)exp(-decay*dist);
vect_scale_inline(&forza, stren * 0.00016f * forceScaleFactor, accel);
}
}
/**
* @brief Mirrors a vector off another, stores results in vector.
*
* @param r Resulting vector of the reflection.
* @param v Vector to reflect.
* @param n Normal to reflect off of.
*/
void vect_reflect( Vector2d* r, Vector2d* v, Vector2d* n )
{
double dot;
dot = vect_dot( v, n );
r->x = v->x - ((2. * dot) * n->x);
r->y = v->y - ((2. * dot) * n->y);
r->mod = MOD(r->x,r->y);
r->angle = MOD(r->x,r->y);
}
void AD_WindModifier::Force (float framepos, AD_Vect3D *pos, AD_Vect3D *vel, AD_Vect3D *accel)
{
AD_Vect3D posrel, tf, p;
float dist, freq, turb, stren;
freq=frequency;
turb=turbolence;
stren=strenght;
if (mapping==FORCE_PLANAR)
{
if (decay!=0.0f)
{
vect_sub_inline(pos, ¤tpos, &posrel);
dist=vect_dot(&forza, &posrel);
dist = fabsf(dist);
stren *= (float)exp(-decay*dist);
}
}
else
{
vect_sub_inline(pos, ¤tpos, &forza);
dist = vect_length(&forza);
if (dist!=0.0f) vect_scale_inline(&forza, 1.0f/dist, &forza);
if (decay!=0.0f)
stren *= (float)exp(-decay*dist);
vect_scale_inline(&forza, stren*0.00016f*forceScaleFactor, &forza);
}
if (turb!=0.0f)
{
vect_sub_inline(pos, ¤tpos, &posrel);
freq *= 0.01f;
turb *= 0.0001f * forceScaleFactor;
p.x = freq * float(framepos);
tf.x = scale*rand()*p.x;
p.y = freq * float(framepos);
tf.y = scale*rand()*p.y;
p.z = freq * float(framepos);
tf.z = scale*rand()*p.z;
vect_scale_inline(&tf, turb, &tf);
vect_add_inline(&forza, &tf, accel);
}
else
{
accel->x=forza.x;
accel->y=forza.y;
accel->z=forza.z;
}
}
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);
}
}
/* Return the angle between two vectors */
float vect_angle (Vector v1, Vector v2)
{
float mag1, mag2, angle, cos_theta;
mag1 = vect_mag(v1);
mag2 = vect_mag(v2);
if (mag1 * mag2 == 0.0)
angle = 0.0;
else {
cos_theta = vect_dot(v1,v2) / (mag1 * mag2);
if (cos_theta <= -1.0)
angle = 180.0;
else if (cos_theta >= +1.0)
angle = 0.0;
else
angle = (180.0/PI) * acos(cos_theta);
}
return angle;
}
/*
Decodes a 3x4 transformation matrix into separate scale, rotation,
translation, and shear vectors. Based on a program by Spencer W.
Thomas (Graphics Gems II)
*/
void mat_decode (Matrix mat, Vector scale, Vector shear, Vector rotate,
Vector transl)
{
int i;
Vector row[3], temp;
for (i = 0; i < 3; i++)
transl[i] = mat[3][i];
for (i = 0; i < 3; i++) {
row[i][X] = mat[i][0];
row[i][Y] = mat[i][1];
row[i][Z] = mat[i][2];
}
scale[X] = vect_mag (row[0]);
vect_normalize (row[0]);
shear[X] = vect_dot (row[0], row[1]);
row[1][X] = row[1][X] - shear[X]*row[0][X];
row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
scale[Y] = vect_mag (row[1]);
vect_normalize (row[1]);
if (scale[Y] != 0.0)
shear[X] /= scale[Y];
shear[Y] = vect_dot (row[0], row[2]);
row[2][X] = row[2][X] - shear[Y]*row[0][X];
row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
shear[Z] = vect_dot (row[1], row[2]);
row[2][X] = row[2][X] - shear[Z]*row[1][X];
row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
scale[Z] = vect_mag (row[2]);
vect_normalize (row[2]);
if (scale[Z] != 0.0) {
shear[Y] /= scale[Z];
shear[Z] /= scale[Z];
}
vect_cross (temp, row[1], row[2]);
if (vect_dot (row[0], temp) < 0.0) {
for (i = 0; i < 3; i++) {
scale[i] *= -1.0;
row[i][X] *= -1.0;
row[i][Y] *= -1.0;
row[i][Z] *= -1.0;
}
}
if (row[0][Z] < -1.0) row[0][Z] = -1.0;
if (row[0][Z] > +1.0) row[0][Z] = +1.0;
rotate[Y] = asin(-row[0][Z]);
if (fabs(cos(rotate[Y])) > EPSILON) {
rotate[X] = atan2 (row[1][Z], row[2][Z]);
rotate[Z] = atan2 (row[0][Y], row[0][X]);
}
else {
rotate[X] = atan2 (row[1][X], row[1][Y]);
rotate[Z] = 0.0;
}
/* Convert the rotations to degrees */
rotate[X] = (180.0/PI)*rotate[X];
rotate[Y] = (180.0/PI)*rotate[Y];
rotate[Z] = (180.0/PI)*rotate[Z];
}
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 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];
}
}
}
}
double vect_magnitude(vect_s v)
{
return sqrt(vect_dot(v, v));
}
/**
* 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;
}
}
