예제 #1
0
파일: ray_throw.c 프로젝트: kube/RT
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);
}
예제 #2
0
파일: pilot_weapon.c 프로젝트: nenau/naev
/**
 * @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;
}
예제 #3
0
파일: physics.c 프로젝트: Anatolis/naev
/**
 * @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);
}
예제 #4
0
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);
	}
}
예제 #5
0
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, &currentpos, &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(&currentpos, 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);
	}
}
예제 #6
0
파일: physics.c 프로젝트: Anatolis/naev
/**
 * @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);
}
예제 #7
0
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, &currentpos, &posrel);
	  dist=vect_dot(&forza, &posrel);
      dist = fabsf(dist);
      stren *= (float)exp(-decay*dist);
	}			
  } 
  else
  {
    vect_sub_inline(pos, &currentpos, &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, &currentpos, &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;
  }
}
예제 #8
0
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);
  }
}
예제 #9
0
/* 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;
}
예제 #10
0
/*
   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];
}
예제 #11
0
파일: crt.c 프로젝트: arader/crt
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;
}
예제 #12
0
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, &currentmatrix, &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(&currentmatrix, &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];
	   }
	}
  }
}
예제 #13
0
파일: vect.c 프로젝트: henkboom/rhizome
double vect_magnitude(vect_s v)
{
    return sqrt(vect_dot(v, v));
}
예제 #14
0
/**
 * 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;
	}
}