void ColorScreening(IplImage *src, IplImage *dst)
{
for (int i = 0; i < src->height; i++) {
uchar* srcp = (uchar*)(src->imageData + i*src->widthStep);
uchar* dstp = (uchar*)(dst->imageData + i*src->widthStep);
for (int j = 0; j < src->width; j++) {
unsigned int b = srcp[3*j];
unsigned int g = srcp[3*j+1];
unsigned int r = srcp[3*j+2];
unsigned int hue = Hue(r, g, b);
float sat = Saturation(r, g, b);
unsigned int ins = Intensity(r, g, b);
//printf("src: %d %d %d\n", r, g, b);
//printf("cal: %d %f %d\n", hue, sat, ins);
if (((hue >= H_MIN1 && hue <= H_MAX1) || (hue >= H_MIN2 && hue <= H_MAX2)) && sat >= S_THRESHOLD && ins >= I_THRESHOLD){
dstp[3*j] = 0;
dstp[3*j+1] = 0;
dstp[3*j+2] = r;
//printf("sto: %d %d %d\n", dstp[3*j], dstp[3*j+1], dstp[3*j+2]);
} else {
dstp[3*j] = 0;
dstp[3*j+1] = 0;
dstp[3*j+2] = 0;
//printf("sto: %d %d %d\n", dstp[3*j], dstp[3*j+1], dstp[3*j+2]);
}
}
}
}
hduVector3Dd Phantom::getForce(float virtualWallSpring,float constraintSpringX = 0.0f,float constrainSpringY = 0.0f) {
if (_pos_z > 0)
_force[1] = 0;
else
_force[1] = -virtualWallSpring*_pos_z;
_force[0] = -constraintSpringX * _pos_x;
_force[2] = -constrainSpringY * _pos_y;
Saturation();
return _force;
}
//-----------------------------------------------------------------------------
bool tIMX51Video::SetSaturationInternal()
{
if(m_pGstSource)
{
GParamSpec *param = g_object_class_find_property(G_OBJECT_GET_CLASS(m_pGstSource), "saturation");
if(!param)
return false;
GParamSpecInt *pint = G_PARAM_SPEC_INT (param);
if(!pint)
return false;
int val = (pint->maximum - pint->minimum) * Saturation() / 100 + pint->minimum;
//qDebug() << "Setting saturation" << val;
g_object_set(m_pGstSource, "saturation", val, NULL);
}
return false;
}
SimpleSound Mix(SimpleSound sound1, SimpleSound sound2)
{
long sampleRate = sound1.GetSampleRate();
long size = sound1.GetSize();
double* data = new double[size];
double* ch1 = sound1.GetPtrData();
double* ch2 = sound2.GetPtrData();
for (int i = 0; i < size; ++i)
{
double sum = ch1[i] + ch2[i];
data[i] = Saturation(sum);
}
SimpleSound result(sampleRate, size, data);
delete[] data;
return result;
}
/**
Check if a point (R,G,B) white
@param R inputR
@param G inputG
@param B inputB
@return 1:white 0:not white
*/
INT8 AwbCheckWhite(UINT R,UINT G,UINT B)
{
UINT _GainSum,_RBGainSum,_RBGainDelta,_DiffRBGain;
UINT _Saturation;
UINT rgain,bgain;
if(G<Thr_LowBright2)
{
return Bright_lo;
}
if(G>Thr_HighBright2)
{
return Bright_hi;
}
rgain=100*G/R;
bgain=100*G/B;
#if 0
if((rgain<125)&&(bgain<115))
return R_BGain_lo;
if( (rgain>135 && bgain > 127) )
return R_BGain_hi;
#endif
_GainSum = rgain + bgain;
_RBGainSum = (100*(R*R+B*B)/(R*B));
_RBGainDelta = abs(_GainSum-_RBGainSum);
_Saturation = Saturation(R,G,B);
_DiffRBGain = (abs(rgain-bgain));
if(AWB_LV<gOutdoor_LV)
{
if(rgain<Thr_GR_L2)
return Rgain_lo;
else if(rgain>Thr_GR_H2)
return Rgain_hi;
else if(bgain<Thr_GB_L2)
return Bgain_lo;
else if(bgain>Thr_GB_H2)
return Bgain_hi;
else if(((((int)R*100)<Thr_RB_L2*(int)B)) )
return RBGain_lo;
else if( ((int)R*100)>Thr_RB_H2*(int)B )
return RBGain_hi;
else if(_GainSum <Thr_GainSum_L2)
return GainSum_lo;
else if(_GainSum > Thr_GainSum_H2)
return GainSum_hi;
else if(_Saturation <Thr_S_L2)
return S_lo;
else if(_Saturation > Thr_S_H2)
return S_hi;
else if(_RBGainSum <Thr_RBSum_L2)
return RBSum_lo;
else if(_RBGainSum > Thr_RBSum_H2)
return RBSum_hi;
else if(_RBGainDelta <Thr_Delta_L2)
return Delta_lo;
else if(_RBGainDelta > Thr_Delta_H2)
return Delta_hi;
}
else
{
if(rgain<Thr_GR_L3)
return Rgain_lo;
else if(rgain>Thr_GR_H3)
return Rgain_hi;
else if(bgain<Thr_GB_L3)
return Bgain_lo;
else if(bgain>Thr_GB_H3)
return Bgain_hi;
else if(((((int)R*100)<Thr_RB_L3*(int)B)) )
return RBGain_lo;
else if( ((int)R*100)>Thr_RB_H3*(int)B )
return RBGain_hi;
else if(_GainSum <Thr_GainSum_L3)
return GainSum_lo;
else if(_GainSum > Thr_GainSum_H2)
return GainSum_hi;
else if(_Saturation <Thr_S_L3)
return S_lo;
else if(_Saturation > Thr_S_H3)
return S_hi;
else if(_RBGainSum <Thr_RBSum_L3)
return RBSum_lo;
else if(_RBGainSum > Thr_RBSum_H2)
return RBSum_hi;
else if(_RBGainDelta <Thr_Delta_L3)
return Delta_lo;
else if(_RBGainDelta > Thr_Delta_H3 )
return Delta_hi;
}
return OK;
}
HDCallbackCode HDCALLBACK phantom_callback(void *pUserData)
{
double Acc=0;
double Mss = 0.0001;
Xprv = X;
hdBeginFrame(hdGetCurrentDevice());
hdGetDoublev(HD_CURRENT_POSITION,posistion);
if (start)
{
X0=posistion[0];
start = false;
hdEndFrame(Device);
return HD_CALLBACK_CONTINUE;
}
X = posistion[0]-X0;
V = X - Xprv;
/*
* Velocity Command channel
*/
// Calculate Positive Energy
if (Fs*Xm>0)
{
mst_Xm_P += Fs*Xm*Scale;
}
else
{
// Do nothing
}
//ROS_INFO("Xm channel Energy: %5f",mst_Xm_P);
/*
* Velocity feedback channel
*/
Fve = -FveGain*(Xm*Scale-Vs);
if (Fve*Vs>0)
{
mst_Vs_N -=Fve*Vs;
}
else
{
//Do nothing
}
//ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Vs_N,Vs,Fve);
//PC
#if (PC_Fve_On)
{
if (mst_Vs_N+sla_Vs_P<0)
{
mst_Vs_N +=Fve*Vs; // backward 1 step
// Modify Vs
if (Fve*Fve>0)
{
Vs = (mst_Vs_N+sla_Vs_P)/Fve;
}
else
{
Vs = 0;
}
// update
Fv = -FveGain*(Xm*Scale-Vs);
mst_Vs_N -=Fv*Vs;
ROS_INFO("PC");
}
}
#endif
/*
* Obstacle force
*/
if (Fe*Vm>0)
{
mst_Fe_N-=Fe*Vm; //fe channel output energy
}
else
{
}
// ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Fe_N,Vm,Fe);
// fprintf(mstEnergy,"%.3f \n",mstE_N_fe);
// fprintf(slvEnergy,"%.3f \n",slaE_P_fe);
// ROS_INFO("Master fe Energy: %5f ",mstE_N_fe);
#if PC_Fe_On
if (mst_Fe_N+sla_Fe_P<0) //output+input <0
{
mst_Fe_N+=Fe*Vm; //backward 1 step
// Modify Fe
if (Vm*Vm > 0.0)
Fe = (mst_Fe_N+sla_Fe_P)/Vm;
else
Fe = 0;
ROS_INFO("PC");
// Update
mst_Fe_N-=Fe*Vm;
}
#endif
/*
* Velocity Based force
*/
if (Fv*Vm>0)
{
mst_Fv_N-=Fv*Vm; //fe channel output energy
}
else
{
}
// ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Fe_N,Vm,Fe);
// fprintf(mstEnergy,"%.3f \n",mstE_N_fe);
// fprintf(slvEnergy,"%.3f \n",slaE_P_fe);
// ROS_INFO("Master fe Energy: %5f ",mstE_N_fe);
#if PC_Fv_On
if (mst_Fv_N+sla_Fv_P<0) //output+input <0
{
mst_Fv_N+=Fv*Vm; //backward 1 step
// Modify Fe
if (Vm*Vm > 0.0)
Fv = (mst_Fv_N+sla_Fv_P)/Vm;
else
Fv = 0;
// Update
mst_Fv_N-=Fv*Vm;
ROS_INFO("PC");
}
#endif
/*
* Combination
*/
#if !FeOn
Fe = 0;
#endif
#if !FveOn
Fve = 0;
#endif
#if !FvOn
Fv = 0;
#endif
#if (PC_Fe_On||PC_Fve_On||PC_Fv_On)
Fm = Fe + Fve + Fv;
Acc = ( -1*K_Master*( Xm - X ) + Fm )/Mss;
Vm += Acc*0.001;
Xm += Vm*0.001;
Fm = K_Master*(Xm - X);
#else
Xm = X;
Vm = V;
Fm = Fe +Fve+Fv;
#endif
// ROS_INFO("Obstacle Feedback Force: %5f",Fe);
// ROS_INFO("Vsd: %5f",Xm*Scale);
ROS_INFO("Force: %5f",Fm);
force[2] = 0;
force[1] = 0;
force[0] = Fm;
Saturation(force);
hdSetDoublev(HD_CURRENT_FORCE, force);
hdEndFrame(hdGetCurrentDevice());
fprintf(data,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",Fm,Fve,Fe,Xm*Scale,Vs,mst_Fe_N,sla_Fe_P,mst_Vs_N,sla_Vs_P,mst_Fv_N,sla_Fv_P,Fv);
MasToSlaDelay();
return HD_CALLBACK_CONTINUE;
}
