//ピクセルシェーダ
float4 PS(VSOutput input):SV_TARGET
{
float3 L=normalize(LightDir);
float3 N=normalize(input.Normal);
float lambertian=saturate(dot(L,N));
float3 V=normalize(-ViewDir);
float3 half_dir=normalize(L+V);
float spec_angle=saturate(dot(half_dir,N));
float specular=pow(spec_angle,Power);
float4 Kt=txDiffuse.Sample(input.Texture);
float4 Kd=LightDiffuse*Diffuse*Kt;
float4 Ks=LightSpecular*float4(Specular,1.0f);
float4 color_linear=LightAmbient*Kd;
float2 texel=float2(
input.Shadow.x/input.Shadow.w*0.5f+0.5f,
input.Shadow.y/input.Shadow.w*-0.5f+0.5f);
if(texel.x<0||texel.x>1||texel.y<0||texel.y>1){
color_linear+=lambertian*Kd+specular*Ks;
}
else{
float shadow=txShadow.Sample(samLinear,texel).r;
if(shadow>=input.Shadow.z/input.Shadow.w-0.0001f){
color_linear+=lambertian*Kd+specular*Ks;
}
}
return saturate(color_linear);
}
bool ofxColorPicker_<ColorType>::mouseUpdate(ofMouseEventArgs& mouse){
if(rectBackground.inside(mouse)){
switch (state) {
case ChangingScale:{
int relY = mouse.y - rectColorScaleBar.y;
float scale = 1.f - saturate(relY / rectColorScaleBar.height);
setColorScale(scale);
setNeedsRedraw();
break;
}
case ChangingWheel:{
auto p = mouse - rectColorWheel.position.xy();
auto pc = getPolarCoordinate(p, colorWheelRadius);
colorAngle = pc.angle;
colorRadius = saturate(pc.radius);
bSettingColor = true;
color = getCircularColor<ColorType>(colorAngle, colorRadius, colorScale);
bSettingColor = false;
setNeedsRedraw();
break;
}
default: return true;
}
return true;
}
return false;
}
double* uavAutopilot(UavStates* uav,autoPilot* pid, double V,double h,double theta,double q,double phi,double p,double xiDV,double xiDh,double xiDtheta,double xiDphi,double phid)
{
//// The Autopilot
// issues control commands
double deg2rad = pi/180;
double rad2deg = 180/pi;
// velocity hold
double dV = uav->V0-V;
double delta_t = saturate(dV*pid->kVP+xiDV*pid->kVI+1.0075,0.8,2);
// delta_t = saturate(dV*pid->kVP+xiDV*pid->kVI,0.8,2);
// altitude hold
double dh = uav->h0-h;
double theta_error = (dh*pid->khP+xiDh*pid->khI-(theta-uav->theta0)*rad2deg)*pid->kthetaP+xiDtheta*pid->kthetaI;
double q_error = theta_error-q*rad2deg*pid->kqP;
double delta_e = saturate(q_error+0.0018*rad2deg,-40,40)*deg2rad;
// attitude hold
double dphi = (phid - phi)*rad2deg;
double p_error = dphi*pid->kphiP+xiDphi*pid->kphiI-p*rad2deg*pid->kpP;
double delta_a = saturate(p_error,-40,40)*deg2rad;
double delta_r = 0;
double arr[4] = {delta_t,delta_e,delta_a,delta_r};
return arr;//{delta_t,delta_e,delta_a,delta_r};
}
void lit_point_spec_range(\n\
float3 in_pos,\n\
float3 in_nor,\n\
float3 cam_vec,\n\
float shin,\n\
float3 l_pos,\n\
float3 l_diff,\n\
float3 l_spec,\n\
float2 l_range,\n\
inout float3 out_diff,\n\
inout float3 out_spec\n\
)\n\
{\n\
float diff_int, spec_int;\n\
float3 dir;\n\
float3 half_vec;\n\
\n\
diff_int = dot(in_nor, dir = normalize(l_pos - in_pos));\n\
if(diff_int > 0.0)\n\
{\n\
float att = 1 - saturate(distance(l_pos, in_pos) * l_range[0]);\n\
\n\
out_diff += l_diff * diff_int * att;\n\
half_vec = normalize(cam_vec + dir);\n\
spec_int = dot(in_nor, half_vec);\n\
out_spec += l_spec * pow(saturate(spec_int), shin) * att;\n\
}\n\
}\n\
void setPixel(const int x, const int y, const float r, const float g, const float b) {
assert(format() == VPX_IMG_FMT_RGB24);
assert(x>=0 && x<width() && y>=0 && y<height());
uint8_t* buf = buffer();
buf[(y*width()+x)*3+0] = uint8_t(saturate(r) * 255.0);
buf[(y*width()+x)*3+1] = uint8_t(saturate(g) * 255.0);
buf[(y*width()+x)*3+2] = uint8_t(saturate(b) * 255.0);
}
coord_type fi_mul(coord_type op1, coord_type op2)
{
coord_type tmp_op1 = saturate(op1);
coord_type tmp_op2 = saturate(op2);
coord_type result = tmp_op1*tmp_op2;
result = result >> FRACTIONAL_BITS;
return result;
}
// get the color
unsigned int RGBSpectrum::GetColor() const
{
unsigned int color = 0;
color |= ((unsigned char)(255.0f*saturate(m_r)))<<16;
color |= ((unsigned char)(255.0f*saturate(m_g)))<<8;
color |= ((unsigned char)(255.0f*saturate(m_b)))<<0;
return color;
}
// ********************************************************************************************************\n\
float4 PSMainNoTexture( PS_INPUT_NO_TEX input ) : SV_Target\n\
{\n\
float3 normal = normalize(input.Norm);\n\
float nDotL = saturate( dot( normal, -normalize(LightDirection.xyz) ) );\n\
float3 eyeDirection = normalize(EyePosition.xyz - input.Position);\n\
float3 HalfVector = normalize( eyeDirection + (-LightDirection.xyz) );\n\
float nDotH = saturate( dot(normal, HalfVector) );\n\
float3 specular = 0.3f * pow(nDotH, 50.0f );\n\
return float4( (nDotL + specular).xxx, 1.0f);\n\
}\n\
// inner product of p1 and p2
void dot_product_fi(data_type p1,data_type p2, coord_type *r)
{
coord_type tmp = 0;
for (uint d=0;d<D;d++) {
coord_type tmp_op1 = saturate(p1.value[d]);
coord_type tmp_op2 = saturate(p2.value[d]);
coord_type tmp_mul = tmp_op1*tmp_op2;
tmp += tmp_mul;
}
*r = (tmp);
}
// ********************************************************************************************************\n\
float4 PSMainNoTexture( PS_INPUT_NO_TEX input ) : SV_Target\n\
{\n\
float3 lightUv = input.LightUv.xyz / input.LightUv.w;\n\
float2 uv = lightUv.xy * 0.5f + 0.5f;\n\
float3 eyeDirection = normalize(input.Position - EyePosition.xyz);\n\
float3 normal = normalize(input.Norm);\n\
float nDotL = saturate( dot( normal, -normalize(LightDirection.xyz) ) );\n\
float3 reflection = reflect( eyeDirection, normal );\n\
float rDotL = saturate(dot( reflection, -LightDirection.xyz ));\n\
float specular = 0.2f * pow( rDotL, 4.0f );\n\
return float4( (nDotL + specular).xxx, 1.0f);\n\
}\n\
int ip_addr_cmp(struct ip_addr const *a, struct ip_addr const *b)
{
if (a->family < b->family) return -1;
else if (a->family > b->family) return 1;
else switch (a->family) {
case AF_INET:
return saturate(memcmp(&a->u.v4, &b->u.v4, sizeof(a->u.v4)));
case AF_INET6:
return saturate(memcmp(&a->u.v6, &b->u.v6, sizeof(a->u.v6)));
}
FAIL("Invalid IP family (%d)", a->family);
return -1;
}
// ********************************************************************************************************\n\
float4 PSMain( PS_INPUT input ) : SV_Target\n\
{\n\
float3 normal = normalize(input.Norm);\n\
float nDotL = saturate( dot( normal, -LightDirection ) );\n\
float3 eyeDirection = normalize(EyePosition.xyz - input.Position);\n\
float3 HalfVector = normalize( eyeDirection + (-LightDirection.xyz) );\n\
float nDotH = saturate( dot(normal, HalfVector) );\n\
float3 specular = 0.3f * pow(nDotH, 50.0f );\n\
float4 diffuseTexture = TEXTURE0.Sample( SAMPLER0, input.Uv );\n\
float ambient = 0.05;\n\
float3 result = (nDotL+ambient) * diffuseTexture + specular;\n\
return float4( result, 1.0f );\n\
}\n\
// inner product of p1 and p2
void dot_product_fi_mixed(data_type_short p1,data_type p2, coord_type *r)
{
data_type tmp_p1 = conv_short_to_long(p1);
coord_type tmp = 0;
for (uint d=0;d<D;d++) {
coord_type tmp_op1 = saturate(tmp_p1.value[d]);
coord_type tmp_op2 = saturate(p2.value[d]);
coord_type tmp_mul = tmp_op1*tmp_op2;
tmp += tmp_mul;
}
*r = (tmp);
}
static void runAddingLowPass(LADSPA_Handle instance, unsigned long sample_count) {
LowPass *plugin_data = (LowPass*)instance;
LADSPA_Data run_adding_gain = plugin_data->run_adding_gain;
/*cutoff (Hz)*/
const LADSPA_Data cutoff = *(plugin_data->m_pfcutoff);
/* Audio input: input1*/
const LADSPA_Data * const input1 = plugin_data->m_pfinput1;
/* Audio input: input2*/
const LADSPA_Data * const input2 = plugin_data->m_pfinput2;
/* Audio input: output1*/
LADSPA_Data * const output1 = plugin_data->m_pfoutput1;
/* Audio input: output2*/
LADSPA_Data * const output2 = plugin_data->m_pfoutput2;
// Parameters last value:
float last_cutoff = plugin_data->last_cutoff;
// States:
float lp2 = plugin_data->lp2;
float lp1 = plugin_data->lp1;
float lambda = plugin_data->lambda;
if (cutoff != last_cutoff) {
// cutoff and samplerate are both in Hertz
lambda = exp(- cutoff / XSPIF_GET_SAMPLE_RATE());
plugin_data->last_cutoff = cutoff;
}
// Here is the DSP algorithm:
{
for(int i=0;i < XSPIF_GET_VECTOR_SIZE();i++)
{
// in and out names are derived from the label in the pin declaration
lp1 = (1.f-lambda)*input1[i] + lambda*lp1;
lp2 = (1.f-lambda)*input2[i] + lambda*lp2;
XSPIF_WRITE_SAMPLE(output1, i, saturate(lp1));
XSPIF_WRITE_SAMPLE(output2, i, saturate(lp2));
}
}
}
void CServer::update_position(u32 robot_idx)
{
u32 i, j;
float position_new[ROBOT_SPACE_DIMENSION];
float tmp_dist = ROBOT_SPACE_DIMENSION;
std::vector<u32> colisions_idx;
i32 wall_idx = -1;
for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
position_new[i] = robots[robot_idx].position[i] + 5.0*robots[robot_idx].d[i]*dt*0.001 + 0.001*rnd_();
for (j = 0; j < robots.size(); j++)
//if ( (j != robot_idx) && (robots[j].type&ROBOT_SOLID_FLAG) )
if ( (j != robot_idx) && (robots[j].type&ROBOT_STRONG_SOLID_FLAG) )
{
tmp_dist = vect_distance(position_new, robots[j].position, ROBOT_SPACE_DIMENSION);
if (tmp_dist < colision_distance)
{
colisions_idx.push_back(j);
if (robots[j].type&ROBOT_STRONG_SOLID_FLAG)
wall_idx = j;
}
}
if (colisions_idx.size() == 0)
//there is no colision
for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
robots[robot_idx].position[i] = saturate(position_new[i], -position_max[i], position_max[i]);
else
{
for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
{
float tmp = 0.0;
if (wall_idx == -1)
tmp = -5.0*robots[robot_idx].d[i]*dt*0.001 + 0.001*rnd_();
else
{
tmp = 0.1*(robots[robot_idx].position[i] - robots[wall_idx].position[i] + 0.001*rnd_());
}
robots[robot_idx].position[i] = saturate(robots[robot_idx].position[i] + tmp, -position_max[i], position_max[i]);
}
}
}
static int16_t gsm_sub (
int16_t a,
int16_t b)
{
int32_t diff = (int32_t)a - (int32_t)b;
return saturate(diff);
}
// ----------------------------------------------------------------------------------------------
Eigen::MatrixXd JacoIKSolver::desiredAngles(geometry_msgs::Pose p_in, JacoAngles JAngle)
{
Eigen::MatrixXd JAngle_out = Eigen::MatrixXd::Zero(1,6);
KDL::JntArray q_out, joint_in;
KDL::Frame f_in;
tf::PoseMsgToKDL(p_in,f_in);
joint_in.resize(6);
joint_in(0) = ( JAngle.Actuator1 - 180.0 ) * DTR;
joint_in(1) = ( JAngle.Actuator2 - 270.0 ) * DTR;
joint_in(2) = ( JAngle.Actuator3 - 90.0 ) * DTR;
joint_in(3) = ( JAngle.Actuator4 - 180.0 ) * DTR;
joint_in(4) = ( JAngle.Actuator5 - 180.0 ) * DTR;
joint_in(5) = ( JAngle.Actuator6 - 270.0 ) * DTR;
ik_solver_->CartToJnt(joint_in, f_in, q_out);
for (int i = 0; i < 6; i++){
JAngle_out(0,i)= saturate(q_out(i) * RTD) ;
}
return JAngle_out;
}
void effectType(const PassRefPtr<CanvasPixelArray>& srcPixelArray, PassRefPtr<ImageData>& imageData, const Vector<float>& values)
{
for (unsigned pixelOffset = 0; pixelOffset < srcPixelArray->length(); pixelOffset++) {
unsigned pixelByteOffset = pixelOffset * 4;
unsigned char r = 0, g = 0, b = 0, a = 0;
srcPixelArray->get(pixelByteOffset, r);
srcPixelArray->get(pixelByteOffset + 1, g);
srcPixelArray->get(pixelByteOffset + 2, b);
srcPixelArray->get(pixelByteOffset + 3, a);
double red = r, green = g, blue = b, alpha = a;
switch (filterType) {
case FECOLORMATRIX_TYPE_MATRIX:
matrix(red, green, blue, alpha, values);
break;
case FECOLORMATRIX_TYPE_SATURATE:
saturate(red, green, blue, values[0]);
break;
case FECOLORMATRIX_TYPE_HUEROTATE:
huerotate(red, green, blue, values[0]);
break;
case FECOLORMATRIX_TYPE_LUMINANCETOALPHA:
luminance(red, green, blue, alpha);
break;
}
imageData->data()->set(pixelByteOffset, red);
imageData->data()->set(pixelByteOffset + 1, green);
imageData->data()->set(pixelByteOffset + 2, blue);
imageData->data()->set(pixelByteOffset + 3, alpha);
}
}
void lit_spot_range(\n\
float3 in_pos,\n\
float3 in_nor,\n\
float3 l_pos,\n\
float3 l_dir,\n\
float inner_cone,\n\
float outer_cone,\n\
float3 l_diff,\n\
float2 l_range,\n\
inout float3 out_diff\n\
)\n\
{\n\
float diff_int, angle, spot_eff;\n\
float3 dir;\n\
\n\
diff_int = dot(in_nor, dir = -l_dir);\n\
if(diff_int > 0.0)\n\
{\n\
angle = dot(normalize(in_pos - l_pos), l_dir);\n\
if(angle > outer_cone)\n\
{\n\
float att = 1 - saturate(distance(in_pos, l_pos) * l_range[0]);\n\
\n\
spot_eff = smoothstep(outer_cone, inner_cone, angle);\n\
out_diff += l_diff * diff_int * spot_eff * att;\n\
}\n\
}\n\
}\n\
void lit_spot_spec(\n\
float3 in_pos,\n\
float3 in_nor,\n\
float3 cam_vec,\n\
float shin,\n\
float3 l_pos,\n\
float3 l_dir,\n\
float inner_cone,\n\
float outer_cone,\n\
float3 l_diff,\n\
float3 l_spec,\n\
inout float3 out_diff,\n\
inout float3 out_spec\n\
)\n\
{\n\
float diff_int, spec_int, angle, spot_eff;\n\
float3 dir;\n\
float3 half_vec;\n\
\n\
diff_int = dot(in_nor, dir = -l_dir);\n\
if(diff_int > 0.0)\n\
{\n\
angle = dot(normalize(in_pos - l_pos), l_dir);\n\
spot_eff = smoothstep(outer_cone, inner_cone, angle);\n\
out_diff += l_diff * diff_int * spot_eff;\n\
half_vec = normalize(cam_vec + dir);\n\
spec_int = dot(in_nor, half_vec);\n\
out_spec += l_spec * pow(saturate(spec_int), shin);\n\
}\n\
}\n\
QPixmap ImageFilter::operator ()(QPixmap sourcePixmap, Filter::FilterType filter, int delta)
{
QImage origin(sourcePixmap.toImage());
switch (filter)
{
case Filter::Gray_Scale:
return grayScale(origin);
break;
case Filter::Brightness:
return brightness(origin, delta);
break;
case Filter::Temperature:
return temperature(origin, delta);
break;
case Filter::Saturation:
return saturate(origin, delta);
break;
case Filter::Blur:
return blur(origin);
break;
case Filter::Sharpen:
return sharpen(origin);
break;
case Filter::Sepia:
return sepia(origin);
break;
}
}
// ********************************************************************************************************\n\
float4 PSMain( PS_INPUT input ) : SV_Target\n\
{\n\
float3 lightUv = input.LightUv.xyz / input.LightUv.w;\n\
lightUv.xy = lightUv.xy * 0.5f + 0.5f; // TODO: Move scale and offset to matrix.\n\
lightUv.y = 1.0f - lightUv.y;\n\
float3 normal = normalize(input.Norm);\n\
float nDotL = saturate( dot( normal, -LightDirection ) );\n\
float3 eyeDirection = normalize(input.Position - EyePosition);\n\
float3 reflection = reflect( eyeDirection, normal );\n\
float rDotL = saturate(dot( reflection, -LightDirection ));\n\
float3 specular = pow(rDotL, 16.0f);\n\
float4 diffuseTexture = TEXTURE0.Sample( SAMPLER0, input.Uv );\n\
float ambient = 0.05;\n\
float3 result = (nDotL+ambient) * diffuseTexture + specular;\n\
return float4( result, 1.0f );\n\
}\n\
float smoothstep(float edge0, float edge1, float x)
{
// Scale, bias and saturate x to 0..1 range
x = saturate( (x-edge0) / (edge1-edge0));
// Evaluate polynomial
return x*x*(3-2*x);
}
vec3 BRDF_BlinnPhong( in vec3 specularColor, in float shininess, in vec3 normal, in vec3 lightDir, in vec3 viewDir ) {
vec3 halfDir = normalize( lightDir + viewDir );
float dotNH = saturate( dot( normal, halfDir ) );
float dotLH = saturate( dot( lightDir, halfDir ) );
vec3 F = F_Schlick( specularColor, dotLH );
float G = G_BlinnPhong_Implicit( /* dotNL, dotNV */ );
float D = D_BlinnPhong( shininess, dotNH );
return F * G * D;
}
float FPR_control::pid(float state, float command, bool flag)
{
if(flag)
{
_integrator = 0.0;
_differentiator = 0.0;
_error_d1 = 0.0;
}
float error = command - state;
if(abs(error) < _intThreshHigh && abs(error) > _intThreshLow)
{
_integrator = _integrator + (_Ts/2.0)*(error + _error_d1);
}
else
{
_integrator = 0.0;
}
_differentiator = ((2.0*_tau - _Ts)/(2.0*_tau + _Ts))*_differentiator + (2.0/(2.0*_tau + _Ts))*(error - _error_d1);
_error_d1 = error;
return(saturate((_kp*error + _ki*_integrator + _kd*_differentiator), 1.0, -1.0));
}
void handleButton()
{
static uint32_t btPrev = 0;
uint32_t bt;
static uint8_t r, g, b;
bt = button();
if (bt)
{
cam_getFrame((uint8_t *)SRAM0_LOC, SRAM0_SIZE, 0x21, 0, 0, 320, 200);
getColor(&r, &g, &b);
saturate(&r, &g, &b);
led_setRGB(r, g, b);
}
else if (btPrev)
{
led_setRGB(0, 0, 0);
delayus(50000);
led_setRGB(r, g, b);
delayus(50000);
led_setRGB(0, 0, 0);
delayus(50000);
led_setRGB(r, g, b);
delayus(50000);
led_setRGB(0, 0, 0);
}
btPrev = bt;
}
void effectType(ByteArray* pixelArray, const Vector<float>& values)
{
unsigned pixelArrayLength = pixelArray->length();
for (unsigned pixelByteOffset = 0; pixelByteOffset < pixelArrayLength; pixelByteOffset += 4) {
double red = pixelArray->get(pixelByteOffset);
double green = pixelArray->get(pixelByteOffset + 1);
double blue = pixelArray->get(pixelByteOffset + 2);
double alpha = pixelArray->get(pixelByteOffset + 3);
switch (filterType) {
case FECOLORMATRIX_TYPE_MATRIX:
matrix(red, green, blue, alpha, values);
break;
case FECOLORMATRIX_TYPE_SATURATE:
saturate(red, green, blue, values[0]);
break;
case FECOLORMATRIX_TYPE_HUEROTATE:
huerotate(red, green, blue, values[0]);
break;
case FECOLORMATRIX_TYPE_LUMINANCETOALPHA:
luminance(red, green, blue, alpha);
break;
}
pixelArray->set(pixelByteOffset, red);
pixelArray->set(pixelByteOffset + 1, green);
pixelArray->set(pixelByteOffset + 2, blue);
pixelArray->set(pixelByteOffset + 3, alpha);
}
}
double tick(double x, double a, double a2, double ainv, double b, double b2, double c, double g, double g0,
double & z0, double & z1, double & z2, double & z3, double & z4)
{
// current state
const double s0 = (a2*a*z0 + a2*b*z1 + z2*(b2 - 2*a2)*a + z3*(b2 - 3*a2)*b) * c;
const double s = bh*s0 - z4;
// solve feedback loop (linear)
double y5 = (g*x + s) / (1 + g*k);
// input clipping
const double y0 = saturate(x - k*y5);
y5 = g*y0 + s;
// compute integrator outputs
const double y4 = g0*y0 + s0;
const double y3 = (b*y4 - z3) * ainv;
const double y2 = (b*y3 - a*y4 - z2) * ainv;
const double y1 = (b*y2 - a*y3 - z1) * ainv;
// update filter state
z0 += 4*a*(y0 - y1 + y2);
z1 += 2*a*(y1 - 2*y2 + y3);
z2 += 2*a*(y2 - 2*y3 + y4);
z3 += 2*a*(y3 - 2*y4);
z4 = bh*y4 + ah*y5;
double result = A * y4;
return result;
}
inline static INT32 asl(INT32 value, int shift, UINT8 &flags) {
INT32 signBefore = value & SIGN_BIT_24;
INT32 result = value << shift;
INT32 signAfter = result & SIGN_BIT_24;
bool overflow = signBefore != signAfter;
flags = setFlagTo(flags, FLAG_V, overflow);
return saturate(result, flags, signBefore != 0);
}
inline static int32_t asl(int32_t value, int shift, uint8_t &flags) {
int32_t signBefore = value & SIGN_BIT_24;
int32_t result = value << shift;
int32_t signAfter = result & SIGN_BIT_24;
bool overflow = signBefore != signAfter;
flags = setFlagTo(flags, FLAG_V, overflow);
return saturate(result, flags, signBefore != 0);
}
