/**
* @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;
}
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);
}
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);
}
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);
}
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;
}
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);
}
void Camera::TurnLeftRight(double angle)
{
LinearCombination(cos(angle), dir, sin(angle), GetLeft(), dir);
Normalize(dir);
ProjectVector(up, dir);
}
void Camera::TiltLeftRight(double angle)
{
LinearCombination(cos(angle), up, sin(angle), GetLeft(), up);
ProjectVector(up, dir);
}
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();
}