Exemple #1
0
/**
*  @brief
*    Project the vector on to the plane created by two direction vectors
*/
Vector4 &Vector4::ProjectPlane(const Vector4 &vV1, const Vector4 &vV2)
{
	Vector4 vT1 = ProjectVector(Vector4::UnitW, vV1);
	Vector4 vT2 = ProjectVector(Vector4::UnitW, vV2);
	x = vT1.x + vT2.x;
	y = vT1.y + vT2.y;
	z = vT1.z + vT2.z;
	w = vT1.w + vT2.w;

	return *this;
}
Exemple #2
0
void Camera::Set(const double cam[])
{
   eye[0] = cam[0]; eye[1] = cam[1]; eye[2] = cam[2];
   dir[0] = cam[3]; dir[1] = cam[4]; dir[2] = cam[5];
   up [0] = cam[6]; up [1] = cam[7]; up [2] = cam[8];
   Normalize(dir);
   ProjectVector(up, dir);
}
Exemple #3
0
void Camera::TurnUpDown(double angle)
{
   double old_dir[3] = { dir[0], dir[1], dir[2] };
   double c = cos(angle), s = sin(angle);
   LinearCombination( c, old_dir, s, up, dir);
   LinearCombination(-s, old_dir, c, up, up);
   Normalize(dir);
   ProjectVector(up, dir);
}
void StVKReducedInternalForces::TestPolynomial(double * q, StVKInternalForces * stVKInternalForces, const char * filenameU)
{
  double * fqPoly = (double*) malloc (sizeof(double) * r);
  double * fqDirect = (double*) malloc (sizeof(double) * r);

  Evaluate(q, fqPoly);

  double * U1;
  int n1,r1;

  if (ReadMatrixFromDisk(filenameU, &n1, &r1, &U1) != 0)
  {
    printf("Error loading file %s\n",filenameU);
    exit(1);
  }

  n1 /= 3;

  printf("\nNumber of vertices is: %d\n",n1);
  printf("Number of nonlinear modes is: %d\n",r1);

  double * u = (double*) malloc (sizeof(double) * 3 * n1);
  double * forces = (double*) malloc (sizeof(double) * 3 * n1);

  // u = U*q
  SynthesizeVector(3*n1, r1, U1, q, u);

  stVKInternalForces->ComputeForces(u,forces);

/*
  printf("g***\n");
  for(int j=0; j<3*n1; j++)
    printf("%G\n",forces[j]);
  printf("g***\n");
*/

  ProjectVector(3*n1,r1,U1,fqDirect,forces);

  int i;

  printf("ViaCubPoly ViaProjection:\n");

  for(i=0; i<r1; i++)
  {
    printf("%G %G\n",fqPoly[i],fqDirect[i]);
  }

  free(u);
  free(forces);

  free(U1);

  free(fqDirect);
  free(fqPoly);
}
Exemple #5
0
static Vector3 CalcTangentOnEdge(const Vector3& edgeDir, const Vector3& surfNormal, const Vector3& vertNormal)
{
	Vector3 binormal, projVN;

	binormal = surfNormal ^ edgeDir;
	
	// project the vertex normal on the plane defined by surfNormal and edgeDir 
	// (which is a plane with normal=surfNormal x edgeDir)
	projVN = ProjectVector(binormal, vertNormal);

    Vector3 tg = binormal ^ projVN;
	tg.normalize();
	return tg;
}
void ReducedStVKCubatureForceModel::InitGravity(double *U)
{
    int n=volumetric_mesh_->getNumVertices();
    double *full_gravity_force=new double[3*n];
    memset(full_gravity_force,0.0,sizeof(double)*3*n);
    double *unit_reduced_gravity_force=new double[r_];
    memset(unit_reduced_gravity_force,0.0,sizeof(double)*r_);
    gravity_force_=new double[r_];
    memset(gravity_force_,0.0,sizeof(double)*r_);
    volumetric_mesh_->computeGravity(full_gravity_force,1.0);
    ProjectVector(3*n,r_,U,unit_reduced_gravity_force,full_gravity_force);
    for(int i=0;i<r_;++i)
        gravity_force_[i] = g_*unit_reduced_gravity_force[i];
    for(int i=0;i<r_;++i)
    std::cout<<gravity_force_[i]<<",";
    delete[] full_gravity_force;
    delete[] unit_reduced_gravity_force;
}
void StVKReducedInternalForces::InitGravity(VolumetricMesh * volumetricMesh_, double * U_)
{
  VolumetricMesh * mesh = volumetricMesh;
  if (volumetricMesh_ != NULL)
    mesh = volumetricMesh_;
  
  double * UB = NULL;
  if (U != NULL)
    UB = U;
  if (U_ != NULL)
    UB = U_;

  if ((mesh == NULL) || (UB ==NULL))
  {
    printf("Error: cannot init gravity. Mesh or basis is not specified.\n");
    return;
  }

  if (reducedGravityForce == NULL)
  {
    int n = mesh->getNumVertices();
    double * gravityForce = (double*) malloc (sizeof(double) * 3 * n);
    unitReducedGravityForce = (double*) malloc (sizeof(double) * r);
    reducedGravityForce = (double*) malloc (sizeof(double) * r);
    mesh->computeGravity(gravityForce, 1.0);
    ProjectVector(3*n, r, UB, unitReducedGravityForce, gravityForce);
    //for(int i=0; i<r; i++)
      //printf("%G\n", unitReducedGravityForce[i]);
    free(gravityForce);
  }

  for(int i=0; i<r; i++)
    reducedGravityForce[i] = g * unitReducedGravityForce[i];

  //printf("Altered gravity to %G\n", g);
}
Exemple #8
0
void DSOUND_Calc3DBuffer(IDirectSoundBufferImpl *dsb)
{
	/* volume, at which the sound will be played after all calcs. */
	D3DVALUE lVolume = 0;
	/* stuff for distance related stuff calc. */
	D3DVECTOR vDistance;
	D3DVALUE flDistance = 0;
	/* panning related stuff */
	D3DVALUE flAngle, flAngle2;
	D3DVECTOR vLeft;
	int i, num_main_speakers;
	float a, ingain;
	/* doppler shift related stuff */

	TRACE("(%p)\n",dsb);

	/* initial buffer volume */
	lVolume = dsb->ds3db_lVolume;
	
	switch (dsb->ds3db_ds3db.dwMode)
	{
		case DS3DMODE_DISABLE:
			TRACE("3D processing disabled\n");
			/* this one is here only to eliminate annoying warning message */
			DSOUND_RecalcVolPan (&dsb->volpan);
			return;
		case DS3DMODE_NORMAL:
			TRACE("Normal 3D processing mode\n");
			/* we need to calculate distance between buffer and listener*/
			vDistance = VectorBetweenTwoPoints(&dsb->device->ds3dl.vPosition, &dsb->ds3db_ds3db.vPosition);
			flDistance = VectorMagnitude (&vDistance);
			break;
		case DS3DMODE_HEADRELATIVE:
			TRACE("Head-relative 3D processing mode\n");
			/* distance between buffer and listener is same as buffer's position */
			vDistance = dsb->ds3db_ds3db.vPosition;
			flDistance = VectorMagnitude (&vDistance);
			break;
	}
	
	if (flDistance > dsb->ds3db_ds3db.flMaxDistance)
	{
		/* some apps don't want you to hear too distant sounds... */
		if (dsb->dsbd.dwFlags & DSBCAPS_MUTE3DATMAXDISTANCE)
		{
			dsb->volpan.lVolume = DSBVOLUME_MIN;
			DSOUND_RecalcVolPan (&dsb->volpan);		
			/* i guess mixing here would be a waste of power */
			return;
		}
		else
			flDistance = dsb->ds3db_ds3db.flMaxDistance;
	}		

	if (flDistance < dsb->ds3db_ds3db.flMinDistance)
		flDistance = dsb->ds3db_ds3db.flMinDistance;
	
	/* attenuation proportional to the distance squared, converted to millibels as in lVolume*/
	lVolume -= log10(flDistance/dsb->ds3db_ds3db.flMinDistance * flDistance/dsb->ds3db_ds3db.flMinDistance)*1000 * dsb->device->ds3dl.flRolloffFactor;
	TRACE("dist. att: Distance = %f, MinDistance = %f => adjusting volume %d to %f\n", flDistance, dsb->ds3db_ds3db.flMinDistance, dsb->ds3db_lVolume, lVolume);

	/* conning */
	/* sometimes it happens that vConeOrientation vector = (0,0,0); in this case angle is "nan" and it's useless*/
	if (dsb->ds3db_ds3db.vConeOrientation.x == 0 && dsb->ds3db_ds3db.vConeOrientation.y == 0 && dsb->ds3db_ds3db.vConeOrientation.z == 0)
	{
		TRACE("conning: cones not set\n");
	}
	else
	{
		D3DVECTOR vDistanceInv;

		vDistanceInv.x = -vDistance.x;
		vDistanceInv.y = -vDistance.y;
		vDistanceInv.z = -vDistance.z;

		/* calculate angle */
		flAngle = AngleBetweenVectorsDeg(&dsb->ds3db_ds3db.vConeOrientation, &vDistanceInv);
		/* if by any chance it happens that OutsideConeAngle = InsideConeAngle (that means that conning has no effect) */
		if (dsb->ds3db_ds3db.dwInsideConeAngle != dsb->ds3db_ds3db.dwOutsideConeAngle)
		{
			/* my test show that for my way of calc., we need only half of angles */
			DWORD dwInsideConeAngle = dsb->ds3db_ds3db.dwInsideConeAngle/2;
			DWORD dwOutsideConeAngle = dsb->ds3db_ds3db.dwOutsideConeAngle/2;
			if (dwOutsideConeAngle == dwInsideConeAngle)
				++dwOutsideConeAngle;

			/* full volume */
			if (flAngle < dwInsideConeAngle)
				flAngle = dwInsideConeAngle;
			/* min (app defined) volume */
			if (flAngle > dwOutsideConeAngle)
				flAngle = dwOutsideConeAngle;
			/* this probably isn't the right thing, but it's ok for the time being */
			lVolume += ((flAngle - dwInsideConeAngle)/(dwOutsideConeAngle - dwInsideConeAngle)) * dsb->ds3db_ds3db.lConeOutsideVolume;
		}
		TRACE("conning: Angle = %f deg; InsideConeAngle(/2) = %d deg; OutsideConeAngle(/2) = %d deg; ConeOutsideVolume = %d => adjusting volume to %f\n",
		       flAngle, dsb->ds3db_ds3db.dwInsideConeAngle/2, dsb->ds3db_ds3db.dwOutsideConeAngle/2, dsb->ds3db_ds3db.lConeOutsideVolume, lVolume);
	}
	dsb->volpan.lVolume = lVolume;

	ingain = pow(2.0, dsb->volpan.lVolume / 600.0) * 0xffff;

	if (dsb->device->pwfx->nChannels == 1)
	{
		dsb->volpan.dwTotalAmpFactor[0] = ingain;
		return;
	}
	
	/* panning */
	if (vDistance.x == 0.0f && vDistance.y == 0.0f && vDistance.z == 0.0f)
		flAngle = 0.0;
	else
	{
		vLeft = VectorProduct(&dsb->device->ds3dl.vOrientFront, &dsb->device->ds3dl.vOrientTop);
		/* To calculate angle to sound source we need to:
		 * 1) Get angle between vDistance and a plane on which angle to sound source should be 0.
		 *    Such a plane is given by vectors vOrientFront and vOrientTop, and angle between vector
		 *    and a plane equals to M_PI_2 - angle between vector and normal to this plane (vLeft in this case).
		 * 2) Determine if the source is behind or in front of us by calculating angle between vDistance
		 *    and vOrientFront.
		 */
		flAngle = AngleBetweenVectorsRad(&vLeft, &vDistance);
		flAngle2 = AngleBetweenVectorsRad(&dsb->device->ds3dl.vOrientFront, &vDistance);
		if (flAngle2 > M_PI_2)
			flAngle = -flAngle;
		flAngle -= M_PI_2;
		if (flAngle < -M_PI)
			flAngle += 2*M_PI;
	}
	TRACE("panning: Angle = %f rad, lPan = %d\n", flAngle, dsb->volpan.lPan);

	/* FIXME: Doppler Effect disabled since i have no idea which frequency to change and how to do it */
if(0)
{
	D3DVALUE flFreq, flBufferVel, flListenerVel;
	/* doppler shift*/
	if (!VectorMagnitude(&dsb->ds3db_ds3db.vVelocity) && !VectorMagnitude(&dsb->device->ds3dl.vVelocity))
	{
		TRACE("doppler: Buffer and Listener don't have velocities\n");
	}
	else if (!(dsb->ds3db_ds3db.vVelocity.x == dsb->device->ds3dl.vVelocity.x &&
	           dsb->ds3db_ds3db.vVelocity.y == dsb->device->ds3dl.vVelocity.y &&
	           dsb->ds3db_ds3db.vVelocity.z == dsb->device->ds3dl.vVelocity.z))
	{
		/* calculate length of ds3db_ds3db.vVelocity component which causes Doppler Effect
		   NOTE: if buffer moves TOWARDS the listener, it's velocity component is NEGATIVE
		         if buffer moves AWAY from listener, it's velocity component is POSITIVE */
		flBufferVel = ProjectVector(&dsb->ds3db_ds3db.vVelocity, &vDistance);
		/* calculate length of ds3dl.vVelocity component which causes Doppler Effect
		   NOTE: if listener moves TOWARDS the buffer, it's velocity component is POSITIVE
		         if listener moves AWAY from buffer, it's velocity component is NEGATIVE */
		flListenerVel = ProjectVector(&dsb->device->ds3dl.vVelocity, &vDistance);
		/* formula taken from Gianicoli D.: Physics, 4th edition: */
		/* FIXME: replace dsb->freq with appropriate frequency ! */
		flFreq = dsb->freq * ((DEFAULT_VELOCITY + flListenerVel)/(DEFAULT_VELOCITY + flBufferVel));
		TRACE("doppler: Buffer velocity (component) = %f, Listener velocity (component) = %f => Doppler shift: %d Hz -> %f Hz\n",
		      flBufferVel, flListenerVel, dsb->freq, flFreq);
		/* FIXME: replace following line with correct frequency setting ! */
		dsb->freq = flFreq;
		DSOUND_RecalcFormat(dsb);
	}
}

	for (i = 0; i < dsb->device->pwfx->nChannels; i++)
		dsb->volpan.dwTotalAmpFactor[i] = 0;

	num_main_speakers = dsb->device->pwfx->nChannels;

	if (dsb->device->lfe_channel != -1) {
		dsb->volpan.dwTotalAmpFactor[dsb->device->lfe_channel] = ingain;
		num_main_speakers--;
	}

	/* adapted from OpenAL's Alc/panning.c */
	for (i = 0; i < num_main_speakers - 1; i++)
	{
		if(flAngle >= dsb->device->speaker_angles[i] && flAngle < dsb->device->speaker_angles[i+1])
		{
			/* Sound is between speakers i and i+1 */
			a = (flAngle-dsb->device->speaker_angles[i]) / (dsb->device->speaker_angles[i+1]-dsb->device->speaker_angles[i]);
			dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[i]] = sqrtf(1.0f-a) * ingain;
			dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[i+1]] = sqrtf(a) * ingain;
			return;
		}
	}

	/* Sound is between last and first speakers */
	if (flAngle < dsb->device->speaker_angles[0]) { flAngle += M_PI*2.0f; }
	a = (flAngle-dsb->device->speaker_angles[i]) / (M_PI*2.0f + dsb->device->speaker_angles[0]-dsb->device->speaker_angles[i]);
	dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[i]] = sqrtf(1.0f-a) * ingain;
	dsb->volpan.dwTotalAmpFactor[dsb->device->speaker_num[0]] = sqrtf(a) * ingain;
}
Exemple #9
0
void DSOUND_Calc3DBuffer(IDirectSoundBufferImpl *dsb)
{
	/* volume, at which the sound will be played after all calcs. */
	D3DVALUE lVolume = 0;
	/* stuff for distance related stuff calc. */
	D3DVECTOR vDistance;
	D3DVALUE flDistance = 0;
	/* panning related stuff */
	D3DVALUE flAngle;
	D3DVECTOR vLeft;
	/* doppler shift related stuff */
#if 0
	D3DVALUE flFreq, flBufferVel, flListenerVel;
#endif

	TRACE("(%p)\n",dsb);

	/* initial buffer volume */
	lVolume = dsb->ds3db_lVolume;
	
	switch (dsb->ds3db_ds3db.dwMode)
	{
		case DS3DMODE_DISABLE:
			TRACE("3D processing disabled\n");
			/* this one is here only to eliminate annoying warning message */
			DSOUND_RecalcVolPan (&dsb->volpan);
			break;
		case DS3DMODE_NORMAL:
			TRACE("Normal 3D processing mode\n");
			/* we need to calculate distance between buffer and listener*/
			vDistance = VectorBetweenTwoPoints(&dsb->ds3db_ds3db.vPosition, &dsb->device->ds3dl.vPosition);
			flDistance = VectorMagnitude (&vDistance);
			break;
		case DS3DMODE_HEADRELATIVE:
			TRACE("Head-relative 3D processing mode\n");
			/* distance between buffer and listener is same as buffer's position */
			flDistance = VectorMagnitude (&dsb->ds3db_ds3db.vPosition);
			break;
	}
	
	if (flDistance > dsb->ds3db_ds3db.flMaxDistance)
	{
		/* some apps don't want you to hear too distant sounds... */
		if (dsb->dsbd.dwFlags & DSBCAPS_MUTE3DATMAXDISTANCE)
		{
			dsb->volpan.lVolume = DSBVOLUME_MIN;
			DSOUND_RecalcVolPan (&dsb->volpan);		
			/* i guess mixing here would be a waste of power */
			return;
		}
		else
			flDistance = dsb->ds3db_ds3db.flMaxDistance;
	}		

	if (flDistance < dsb->ds3db_ds3db.flMinDistance)
		flDistance = dsb->ds3db_ds3db.flMinDistance;
	
	/* attenuation proportional to the distance squared, converted to millibels as in lVolume*/
	lVolume -= log10(flDistance/dsb->ds3db_ds3db.flMinDistance * flDistance/dsb->ds3db_ds3db.flMinDistance)*1000;
	TRACE("dist. att: Distance = %f, MinDistance = %f => adjusting volume %d to %f\n", flDistance, dsb->ds3db_ds3db.flMinDistance, dsb->ds3db_lVolume, lVolume);

	/* conning */
	/* sometimes it happens that vConeOrientation vector = (0,0,0); in this case angle is "nan" and it's useless*/
	if (dsb->ds3db_ds3db.vConeOrientation.x == 0 && dsb->ds3db_ds3db.vConeOrientation.y == 0 && dsb->ds3db_ds3db.vConeOrientation.z == 0)
	{
		TRACE("conning: cones not set\n");
	}
	else
	{
		/* calculate angle */
		flAngle = AngleBetweenVectorsDeg(&dsb->ds3db_ds3db.vConeOrientation, &vDistance);
		/* if by any chance it happens that OutsideConeAngle = InsideConeAngle (that means that conning has no effect) */
		if (dsb->ds3db_ds3db.dwInsideConeAngle != dsb->ds3db_ds3db.dwOutsideConeAngle)
		{
			/* my test show that for my way of calc., we need only half of angles */
			DWORD dwInsideConeAngle = dsb->ds3db_ds3db.dwInsideConeAngle/2;
			DWORD dwOutsideConeAngle = dsb->ds3db_ds3db.dwOutsideConeAngle/2;
			if (dwOutsideConeAngle == dwInsideConeAngle)
				++dwOutsideConeAngle;

			/* full volume */
			if (flAngle < dwInsideConeAngle)
				flAngle = dwInsideConeAngle;
			/* min (app defined) volume */
			if (flAngle > dwOutsideConeAngle)
				flAngle = dwOutsideConeAngle;
			/* this probably isn't the right thing, but it's ok for the time being */
			lVolume += ((dsb->ds3db_ds3db.lConeOutsideVolume)/((dwOutsideConeAngle) - (dwInsideConeAngle))) * flAngle;
		}
		TRACE("conning: Angle = %f deg; InsideConeAngle(/2) = %d deg; OutsideConeAngle(/2) = %d deg; ConeOutsideVolume = %d => adjusting volume to %f\n",
		       flAngle, dsb->ds3db_ds3db.dwInsideConeAngle/2, dsb->ds3db_ds3db.dwOutsideConeAngle/2, dsb->ds3db_ds3db.lConeOutsideVolume, lVolume);
	}
	dsb->volpan.lVolume = lVolume;
	
	/* panning */
	if (dsb->device->ds3dl.vPosition.x == dsb->ds3db_ds3db.vPosition.x &&
	    dsb->device->ds3dl.vPosition.y == dsb->ds3db_ds3db.vPosition.y &&
	    dsb->device->ds3dl.vPosition.z == dsb->ds3db_ds3db.vPosition.z) {
		dsb->volpan.lPan = 0;
		flAngle = 0.0;
	}
	else
	{
		vDistance = VectorBetweenTwoPoints(&dsb->device->ds3dl.vPosition, &dsb->ds3db_ds3db.vPosition);
		vLeft = VectorProduct(&dsb->device->ds3dl.vOrientFront, &dsb->device->ds3dl.vOrientTop);
		flAngle = AngleBetweenVectorsRad(&vLeft, &vDistance);
		/* for now, we'll use "linear formula" (which is probably incorrect); if someone has it in book, correct it */
		dsb->volpan.lPan = 10000*2*flAngle/M_PI - 10000;
	}
	TRACE("panning: Angle = %f rad, lPan = %d\n", flAngle, dsb->volpan.lPan);

	/* FIXME: Doppler Effect disabled since i have no idea which frequency to change and how to do it */
#if 0	
	/* doppler shift*/
	if ((VectorMagnitude(&ds3db_ds3db.vVelocity) == 0) && (VectorMagnitude(&dsb->device->ds3dl.vVelocity) == 0))
	{
		TRACE("doppler: Buffer and Listener don't have velocities\n");
	}
	else if (ds3db_ds3db.vVelocity != dsb->device->ds3dl.vVelocity)
	{
		/* calculate length of ds3db_ds3db.vVelocity component which causes Doppler Effect
		   NOTE: if buffer moves TOWARDS the listener, it's velocity component is NEGATIVE
		         if buffer moves AWAY from listener, it's velocity component is POSITIVE */
		flBufferVel = ProjectVector(&dsb->ds3db_ds3db.vVelocity, &vDistance);
		/* calculate length of ds3dl.vVelocity component which causes Doppler Effect
		   NOTE: if listener moves TOWARDS the buffer, it's velocity component is POSITIVE
		         if listener moves AWAY from buffer, it's velocity component is NEGATIVE */
		flListenerVel = ProjectVector(&dsb->device->ds3dl.vVelocity, &vDistance);
		/* formula taken from Gianicoli D.: Physics, 4th edition: */
		/* FIXME: replace dsb->freq with appropriate frequency ! */
		flFreq = dsb->freq * ((DEFAULT_VELOCITY + flListenerVel)/(DEFAULT_VELOCITY + flBufferVel));
		TRACE("doppler: Buffer velocity (component) = %lf, Listener velocity (component) = %lf => Doppler shift: %ld Hz -> %lf Hz\n", flBufferVel, flListenerVel,
		      dsb->freq, flFreq);
		/* FIXME: replace following line with correct frequency setting ! */
		dsb->freq = flFreq;
		DSOUND_RecalcFormat(dsb);
		DSOUND_MixToTemporary(dsb, 0, dsb->buflen);
	}
#endif	
	
	/* time for remix */
	DSOUND_RecalcVolPan(&dsb->volpan);
}
Exemple #10
0
void Camera::TurnLeftRight(double angle)
{
   LinearCombination(cos(angle), dir, sin(angle), GetLeft(), dir);
   Normalize(dir);
   ProjectVector(up, dir);
}
Exemple #11
0
void Camera::TiltLeftRight(double angle)
{
   LinearCombination(cos(angle), up, sin(angle), GetLeft(), up);
   ProjectVector(up, dir);
}
Exemple #12
0
void Reaching::ReachingStep(float dt){
    if(dt<EPSILON) return;//no point of doing anything
    //  cout<<"DT "<<dt<<endl;
    joint_vec_t tmp1,tmp3; // temporary variables
    cart_vec_t tmp13;
    float dist=0;
  
    // dt = dt/10; 
    //dist = UpdateLocalTarget();
    if(dist>tol || isnan(dist)){
        SetLocalTarget(target);
        cout<<dist<<" "<<tol<<endl;
    }
    // vite in angle space and cart space
    alpha*=dt;beta*=dt;
    ViteAngle(tar_angle,pos_angle,v_angle,des_angle,des_v_angle);
    ViteCart(target,pos_cart,v_cart,des_cart,des_v_cart);
    alpha /= dt; beta/=dt;

    //  cout<<"vite done"<<endl; 
#ifdef OBSTACLE_AVOIDANCE
    // find the closest obstacle
    float ro,point;
    float gamma;
    CMatrix4_t ijac;
    v4_clear(des_a_angle_obs);
    if(env){
        for (i=1;i<3;i++){
            for(int j=0;j<env->CountManipulableObjects();j++){
                LinkDistanceToObject(i,env->GetObject(j),&ro,&point,tmp13);
                //      coutvec(tmp13);
                IntermediateJacobian(i,point,pos_angle,ijac);
                m3_4_t_v_multiply(ijac,tmp13,tmp1);
                // eq. 18 of Khatib,icra'85
                if(ro<=obstacle_rad){
                    gamma = nu *(1/ro -1/obstacle_rad)/(ro*ro); //(ro*ro) 
                    //test
                    //   gamma *= -v4_dot(tmp1,v_angle)/50;
                    //   gamma = max(0.0,gamma);
                }
                else{
                    gamma =0;
                }
                v4_scale(tmp1,gamma,tmp1);
                v4_add(des_a_angle_obs,tmp1,des_a_angle_obs);
            }
        }
        v4_add(des_a_angle_obs,des_v_angle,des_v_angle);
        v4_add(pos_angle,des_v_angle,des_angle);
    }
#endif
 
    body->SetAnglesInRange(des_angle);
    des_v_angle = des_angle - pos_angle;
    des_v_cart = des_cart - pos_cart;
    if(pure_joint_ctl){
        tmp1 = des_angle;
    }
    else{
        UpdateWeights();
        // coherence enforcement
        ProjectVector(des_v_cart,des_v_angle,tmp3);
        tmp1 = pos_angle+tmp3;
    }
    body->SetAnglesInRange(tmp1);
    body->Angle2Cart(tmp1,tmp13);
    v_angle = tmp1 - pos_angle;
    v_cart = tmp13 - pos_cart;
    pos_cart = tmp13;
    pos_angle = tmp1;
    //pos_angle.Print();
    //pos_cart.Print();
}