void p_spark_colorful_create(VECTOR *position) { if(!position)position = nullvector; wait(3); you = ent_create(NULL,position,emit_colorfulspark); if(you) { vec_add(you.x,vector(60.425,91.578,0.000)); vec_set(you.pan,vector(0.000,0.000,0.000)); vec_set(you.scale_x,vector(1.000,1.000,1.000)); set(you,PASSABLE); set(you,INVISIBLE); } }
static void update_system( m_elem *z, m_elem *x_pre, m_elem **K, m_elem *x_post ) { #ifdef PRINT_DEBUG printf( "ekf: updating system\n" ); #endif apply_measurement( x_pre, z_estimate ); vec_sub( z, z_estimate, z_estimate, measurement_size ); mat_mult_vector( K, z_estimate, x_post, state_size, measurement_size ); vec_add( x_post, x_pre, x_post, state_size ); }
void pFountain(PARTICLE *p) { VECTOR vecTemp; vec_randomize(vecTemp, 2); vec_add(p.vel_x, vecTemp); vec_set(p.blue, vector(random(255), random(255), 255)); set(p, MOVE | BRIGHT | TRANSLUCENT); p.alpha = 100; p.size = 2; p.gravity = 0.2; p.skill_a = 3; p.event = pAlphaFade; }
void p_composition_create(VECTOR *position) { if(!position)position = nullvector; wait(3); you = ent_create(NULL,position,emit_composition); if(you) { vec_add(you.x,vector(-141.194,16.143,0.000)); vec_set(you.pan,vector(0.000,0.000,0.000)); vec_set(you.scale_x,vector(1.000,1.000,1.000)); set(you,PASSABLE); set(you,INVISIBLE); } }
void p_double_helix_create(VECTOR *position) { if(!position)position = nullvector; wait(3); you = ent_create(NULL,position,emit_doublehelix); if(you) { vec_add(you.x,vector(-149.688,31.689,0.000)); vec_set(you.pan,vector(0.000,0.000,0.000)); vec_set(you.scale_x,vector(1.000,1.000,1.000)); set(you,PASSABLE); set(you,INVISIBLE); } }
void p_fire_1_create(VECTOR *position) { if(!position)position = nullvector; wait(3); you = ent_create(NULL,position,emit_fire1); if(you) { vec_add(you.x,vector(-27.054,186.542,14.000)); vec_set(you.pan,vector(32.498,-25.617,-3.110)); vec_set(you.scale_x,vector(1.000,1.000,1.000)); set(you,PASSABLE); set(you,INVISIBLE); } }
void pDenseSmoke(PARTICLE *p) { VECTOR vecTemp; vec_randomize(vecTemp, 2); vec_add(p.vel_x, vector(-1+random(2), -1+random(2), 1)); vec_set(p.blue, vector(140, 140, 140)); set(p, MOVE | TRANSLUCENT); p.alpha = 60 + random(20); p.size = 20; p.gravity = 0; p.skill_a = 1; p.event = pAlphaFade; }
void p_space_hole_create(VECTOR *position) { if(!position)position = nullvector; wait(3); you = ent_create(NULL,position,emit_spacehole); if(you) { vec_add(you.x,vector(414.936,660.420,69.408)); vec_set(you.pan,vector(0.000,0.000,0.000)); vec_set(you.scale_x,vector(1.000,1.000,1.000)); set(you,PASSABLE); set(you,INVISIBLE); } }
vprop_t vprop_animate(vprop_t vp, float dt){ vec_t s; if (!(vp.speed == 0.0 || vec_equal(vp.val,vp.target))){ s = vec_scale(vec_normalize(vec_diff(vp.val,vp.target)),vp.speed); if(vec_dist(vp.val,vp.target) < vec_len(s)){ vp.val = vp.target; }else{ vp.val = vec_add(vp.val,s); } } return vp; }
static void ahrs_update_imu(ahrs_t *ahrs, real_t gx, real_t gy, real_t gz, real_t ax, real_t ay, real_t az, real_t dt) { /* get rate of change of quaternion from gyroscope: */ vec4_t qdot; vec4_init(&qdot); qdot.ve[0] = REAL(0.5) * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - ahrs->quat.q3 * gz); qdot.ve[1] = REAL(0.5) * ( ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - ahrs->quat.q3 * gy); qdot.ve[2] = REAL(0.5) * ( ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + ahrs->quat.q3 * gx); qdot.ve[3] = REAL(0.5) * ( ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - ahrs->quat.q2 * gx); if (!((ax == REAL(0.0)) && (ay == REAL(0.0)) && (az == REAL(0.0)))) { /* normalize accelerometer measurements: */ ahrs_normalize_3(&ax, &ay, &az); /* auxiliary variables to avoid repeated arithmetic: */ real_t _2q0 = REAL(2.0) * ahrs->quat.q0; real_t _2q1 = REAL(2.0) * ahrs->quat.q1; real_t _2q2 = REAL(2.0) * ahrs->quat.q2; real_t _2q3 = REAL(2.0) * ahrs->quat.q3; real_t _4q0 = REAL(4.0) * ahrs->quat.q0; real_t _4q1 = REAL(4.0) * ahrs->quat.q1; real_t _4q2 = REAL(4.0) * ahrs->quat.q2; real_t _8q1 = REAL(8.0) * ahrs->quat.q1; real_t _8q2 = REAL(8.0) * ahrs->quat.q2; real_t q0q0 = ahrs->quat.q0 * ahrs->quat.q0; real_t q1q1 = ahrs->quat.q1 * ahrs->quat.q1; real_t q2q2 = ahrs->quat.q2 * ahrs->quat.q2; real_t q3q3 = ahrs->quat.q3 * ahrs->quat.q3; /* gradient decent algorithm corrective step: */ vec4_t s; vec4_init(&s); s.ve[0] = _4q0 * q2q2 - _2q2 * ax + _4q0 * q1q1 + _2q1 * ay; s.ve[1] = _4q1 * q3q3 + _2q3 * ax + REAL(4.0) * q0q0 * ahrs->quat.q1 + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 - _4q1 * az; s.ve[2] = REAL(4.0) * q0q0 * ahrs->quat.q2 - _2q0 * ax + _4q2 * q3q3 + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 - _4q2 * az; s.ve[3] = REAL(4.0) * q1q1 * ahrs->quat.q3 + _2q1 * ax + REAL(4.0) * q2q2 * ahrs->quat.q3 + _2q2 * ay; vec_normalize(&s); /* apply feedback step: */ vec_scalar_mul(&s, &s, ahrs->beta); vec_sub(&qdot, &qdot, &s); } /* integrate rate of change to yield quaternion: */ vec_scalar_mul(&qdot, &qdot, dt); vec_add(&ahrs->quat, &ahrs->quat, &qdot); /* normalise quaternion: */ quat_normalize(&ahrs->quat); }
void item_particle (PARTICLE *p) { VECTOR vecTemp; vec_randomize(&vecTemp, 10); vec_normalize(&vecTemp, 1); vec_add (&p->vel_x, &vecTemp); vec_set(&p->blue, vector(0, 150, 150)); set(p, MOVE | TRANSLUCENT /*| BRIGHT*/); p->lifespan = 80; p->size = 15 + random(5); p->event = item_particleFader; }
void p_fountain_1_create(VECTOR *position) { if(!position)position = nullvector; wait(3); you = ent_create(NULL,position,emit_fountain1); if(you) { vec_add(you.x,vector(-200.174,290.466,0.000)); vec_set(you.pan,vector(0.000,0.000,0.000)); vec_set(you.scale_x,vector(1.000,1.000,1.000)); set(you,PASSABLE); set(you,INVISIBLE); } }
void p_spiral_create(VECTOR *position) { if(!position)position = nullvector; wait(3); you = ent_create(NULL,position,emit_spiral); if(you) { vec_add(you.x,vector(371.551,349.278,0.000)); vec_set(you.pan,vector(0.000,0.000,0.000)); vec_set(you.scale_x,vector(1.000,1.000,1.000)); set(you,PASSABLE); set(you,INVISIBLE); } }
static void PmlTableGen(int bw, int m, double *storeplm, double *workspace) { double *prev, *prevprev, *temp1, *temp2, *temp3, *temp4, *x_i, *eval_args; int i; /* double *storeplm_ptr; */ prevprev = workspace; prev = prevprev + (2*bw); temp1 = prev + (2*bw); temp2 = temp1 + (2*bw); temp3 = temp2 + (2*bw); temp4 = temp3 + (2*bw); x_i = temp4 + (2*bw); eval_args = x_i + (2*bw); /* storeplm_ptr = storeplm; */ /* get the evaluation nodes */ EvalPts(2*bw,x_i); ArcCosEvalPts(2*bw,eval_args); /* set initial values of first two Pmls */ for (i=0; i<2*bw; i++) prevprev[i] = 0.0; if (m == 0) for (i=0; i<2*bw; i++) { /* prev[i] = 0.5; */ prev[i] = 0.707106781186547 ; /* 1/sqrt(2) */ } else Pmm_L2(m, eval_args, 2*bw, prev); memcpy(storeplm, prev, (size_t) sizeof(double) * 2 * bw); for(i = 0; i < bw - m - 1; i++) { vec_mul(L2_cn(m,m+i),prevprev,temp1,2*bw); vec_pt_mul(prev, x_i, temp2, 2*bw); vec_mul(L2_an(m,m+i), temp2, temp3, 2*bw); vec_add(temp3, temp1, temp4, 2*bw); /* temp4 now contains P(m,m+i+1) */ storeplm += (2 * bw); memcpy(storeplm, temp4, (size_t) sizeof(double) * 2 * bw); memcpy(prevprev, prev, (size_t) sizeof(double) * 2 * bw); memcpy(prev, temp4, (size_t) sizeof(double) * 2 * bw); } /* storeplm = storeplm_ptr; */ }
/* Computes metallic reflection */ static void ga_shade_reflect(vec_t *color,const ga_scene_t *s, const ga_material_t *mat,const vec_t *pos, const vec_t *dir, const vec_t *norm, float importance, int max_rec){ vec_t r,d1,d2,dr; vec_t rcolor; int sample = mat->soft_ref_sample; int i; r = vec_sub(*dir,vec_scale(2.0f*vec_dot(*norm,*dir),*norm)); vec_fperp(&r,&d1,&d2); if(sample <= 0){ sample = 1; } i = sample; while(i--){ dr = r; vec_ffadd(&dr,mat->soft_ref_angle*(2.0f*random()*RAND_NORM) -1.0f,&d1); vec_ffadd(&dr,mat->soft_ref_angle*(2.0f*random()*RAND_NORM) -1.0f,&d2); vec_fnorm(&dr); rcolor = vec_add(rcolor, vec_scale(mat->ref_factor,vec_mult(mat->ref_color, ga_ray_trace(s,*pos,dr,importance,max_rec -1, NULL)))); } *color = vec_add(*color,vec_scale(1.0f/sample,rcolor)); }
static void vorbis_inverse_coupling_altivec(float *mag, float *ang, intptr_t blocksize) { int i; vector float m, a; vector bool int t0, t1; const vector unsigned int v_31 = //XXX vec_add(vec_add(vec_splat_u32(15),vec_splat_u32(15)),vec_splat_u32(1)); for (i = 0; i < blocksize; i += 4) { m = vec_ld(0, mag+i); a = vec_ld(0, ang+i); t0 = vec_cmple(m, (vector float)vec_splat_u32(0)); t1 = vec_cmple(a, (vector float)vec_splat_u32(0)); a = vec_xor(a, (vector float) vec_sl((vector unsigned int)t0, v_31)); t0 = (vector bool int)vec_and(a, t1); t1 = (vector bool int)vec_andc(a, t1); a = vec_sub(m, (vector float)t1); m = vec_add(m, (vector float)t0); vec_stl(a, 0, ang+i); vec_stl(m, 0, mag+i); } }
void rgbaint_t::blend(const rgbaint_t& other, UINT8 factor) { const VECU32 shift = vec_splat_u32(-16); const VECS32 scale1 = { factor, factor, factor, factor }; const VECS32 scale2 = { 0x100 - factor, 0x100 - factor, 0x100 - factor, 0x100 - factor, }; VECU32 temp = vec_msum((VECU16)m_value, (VECU16)vec_rl(scale1, shift), vec_splat_u32(0)); temp = vec_msum((VECU16)other.m_value, (VECU16)vec_rl(scale2, shift), temp); m_value = vec_msum((VECU16)m_value, (VECU16)scale1, vec_mulo((VECU16)other.m_value, (VECU16)scale2)); m_value = vec_add(vec_sl(temp, shift), (VECU32)m_value); sra(8); }
static void kalman_predict(kalman_t *kf, float a) { /* x = A * x + B * u */ kf->u.ve[0] = a; mat_vec_mul(&kf->t0, &kf->A, &kf->x); /* t0 = A * x */ mat_vec_mul(&kf->t1, &kf->B, &kf->u); /* t1 = B * u */ vec_add(&kf->x, &kf->t0, &kf->t1); /* x = t0 + t1 */ /* P = A * P * AT + Q */ mat_mul(&kf->T0, &kf->A, &kf->P); /* T0 = A * P */ mmtr_mul(&kf->T1, &kf->T0, &kf->A); /* T1 = T0 * AT */ mat_add(&kf->P, &kf->T1, &kf->Q); /* P = T1 * Q */ }
int shadow(vec pt, light l, sphere_list *ss) { vec nudge = vec_scale(0.0001, l.direction); vec lifted = vec_add(pt, nudge); ray test_ray = {lifted, l.direction}; while (ss!=NULL) { hit_test h = intersect(test_ray, ss->s); if (!h.miss) return 1; ss = ss->next; } return 0; }
static Elem * new_term_string(Grammar *g, char *s, char *e, Rule *r) { Term *t = new_term(); Elem *elem; t->string = reinterpret_cast<char*>(MALLOC(e - s + 1)); memcpy(t->string, s, e - s); t->string[e - s] = 0; t->string_len = e - s; vec_add(&g->terminals, t); elem = new_elem_term(t, r); return elem; }
/* Computes the phong hilight */ static void ga_shade_phong(vec_t *color, const ga_material_t *mat,const vec_t *lcolor, const vec_t *ldir, const vec_t *dir, const vec_t *norm, float factor){ vec_t r; float fact; float power; r = vec_sub( vec_scale(2.0f,vec_scale(vec_dot(*norm,*ldir),*norm)), *ldir); if((fact = vec_dot(r,vec_neg(*dir))) > 0.0f){ power = powf(fact,mat->spec_power)*mat->spec_factor; *color = vec_add(*color, vec_scale( power*factor, vec_mult(mat->spec_color,*lcolor))); } }
static void add_int16_altivec(int16_t * v1, int16_t * v2, int order) { int i; register vec_s16_t vec, *pv; for(i = 0; i < order; i += 8){ pv = (vec_s16_t*)v2; vec = vec_perm(pv[0], pv[1], vec_lvsl(0, v2)); vec_st(vec_add(vec_ld(0, v1), vec), 0, v1); v1 += 8; v2 += 8; } }
void pStars(PARTICLE *p) { VECTOR vecTemp; vec_randomize(vecTemp, -1); vec_add(p.vel_x, vecTemp); vec_set(p.blue, vector(random(255), random(255), 255)); set(p, MOVE | BRIGHT | TRANSLUCENT); p.alpha = 100; p.size = 3; p.gravity = 1; p.skill_a = 3; p.bmap = bmapStar; p.event = pAlphaFade; }
void update_tick_avatar(){ // boost when player wants to boost, has boost and not currently boosting Bool can_boost = !tick_boost_ttl and tick_energie; if(input_key_down(INPUT_KEY_UP) and can_boost){ synth_play_channel4(SYNTH_NOTE_CIS, SYNTH_OCTAVE_C0); tick_boost_vec = vec_make_float( 0.00, - BOOST_ACCEL); tick_boost_ttl = BOOST_TIME; tick_energie--; } else if(input_key_down(INPUT_KEY_DOWN) and can_boost){ synth_play_channel4(SYNTH_NOTE_CIS, SYNTH_OCTAVE_C0); tick_boost_vec = vec_make_float( 0.00, BOOST_ACCEL); tick_boost_ttl = BOOST_TIME; tick_energie--; } else if(input_key_down(INPUT_KEY_A) and can_boost){ synth_play_channel4(SYNTH_NOTE_AIS, SYNTH_OCTAVE_C0); if (tick_energie > 1){ tick_boost_vec = vec_make_float(BOOST_ACCEL + BOOST_ACCEL, 0.00); tick_boost_ttl = BOOST_TIME; tick_energie -= 2; } else { tick_boost_vec = vec_make_float(BOOST_ACCEL, 0.00); tick_boost_ttl = BOOST_TIME; tick_energie--; } } // move tick_avatar if (tick_boost_ttl){ tick_avatar.vec = vec_add(tick_avatar.vec, tick_boost_vec); tick_boost_ttl--; } tick_avatar.pos = vec_add(tick_avatar.pos, tick_avatar.vec); if (tick_avatar.hit_ttl){ tick_avatar.hit_ttl--; } }
int init_env(t_env *e, t_obj *obj) { if ((e->mlx = mlx_init()) == NULL) return (m_error("mlx_init(): fail")); if ((e->win = mlx_new_window(e->mlx, WIN_X, WIN_Y, "rtv1")) == NULL) return (m_error("mlx_new_window(): fail")); if ((e->img = mlx_new_image(e->mlx, WIN_X, WIN_Y)) == NULL) return (m_error("mlx_new_image(): fail")); if ((e->addr = mlx_get_data_addr(e->img, &(e->bpp), &(e->size_line), &(e->endian))) == NULL) return (m_error("mlx_get_data_addr(): fail")); if ((e->rgb_tab = (t_rgb *)malloc(sizeof(t_rgb) * (WIN_X * WIN_Y))) == NULL) return (m_error("rgb_tab_init(): fail")); e->eye_pos = vec_new(0, 0, 0); e->eye_dir = vec_new(0, 0, 1); e->right_vec = vec_new(1, 0, 0); e->up_vec = vec_new(0, -1, 0); e->view_plane_ori = vec_add(vec_add(e->eye_pos, vec_numb(e->eye_dir, VIEW_PLANE_DIST)), vec_sub(vec_numb(e->up_vec, VIEW_PLANE_HEIGHT / 2.0) , vec_numb(e->right_vec, VIEW_PLANE_WIDTH / 2.0))); init_obj(obj); return (0); }
Production * new_production(Grammar *g, char *name) { Production *p; if ((p = lookup_production(g, name, strlen(name)))) { FREE(name); return p; } p = reinterpret_cast<Production*>(MALLOC(sizeof(Production))); memset(p, 0, sizeof(Production)); vec_add(&g->productions, p); p->name = name; p->name_len = strlen(name); return p; }
void BaseStateEstimation::transformImuStateToBase() { MY_MATRIX(S_angrate, 1,3,1,3); vectorToSkewMatrix(O_angrate_I_, S_angrate); MY_MATRIX(S_angacc, 1,3,1,3); vectorToSkewMatrix(O_angacc_I_, S_angacc); MY_MATRIX(S_helper, 1,3,1,3); mat_mult(S_angrate, S_angrate, S_helper); mat_add(S_angacc, S_helper, S_helper); mat_mult(O_rot_I_, I_rot_B_, O_rot_B_); mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_); vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_); mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_); mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_); vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_); mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_); mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_); vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_); }
/* this is an alternate version of set_dry_signal() in reverb.c */ void v_set_dry_signal(void* destp, const int32* buf, int32 n) { int i = 0; if( n>=8 ) { vint32* dest = (vint32*) destp; for( ; i < n / 4; i++) { dest[i] = vec_add(dest[i],((vint32*) buf)[i]); } } /* write remaining few bytes. */ for( i *= 4; i < n; i++) { ((int32*) destp)[i] += buf[i]; } }
/** main navigation routine. This is called periodically evaluates the current Position and stage and navigates accordingly. Returns True until the survey is finished **/ bool_t poly_survey_adv(void) { //entry circle around entry-center until the desired altitude is reached if (psa_stage == ENTRY) { nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad); if (NavCourseCloseTo(segment_angle) && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT) && fabs(estimator_z - psa_altitude) <= 20) { psa_stage = SEG; NavVerticalAutoThrottleMode(0.0); nav_init_stage(); //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5); } } //fly the segment until seg_end is reached if (psa_stage == SEG) { nav_points(seg_start, seg_end); //calculate all needed points for the next flyover if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) { //dc_stop(); VEC_CALC(seg_center1, seg_end, rad_vec, -); ret_start.x = seg_end.x - 2*rad_vec.x; ret_start.y = seg_end.y - 2*rad_vec.y; //if we get no intersection the survey is finished if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec))) return FALSE; ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x; ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y; seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x); seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y); psa_stage = TURN1; nav_init_stage(); }
void ConditionalSquareSum(const uint8_t * src, size_t srcStride, size_t width, size_t height, const uint8_t * mask, size_t maskStride, uint8_t value, uint64_t * sum) { assert(width >= A); if (align) assert(Aligned(src) && Aligned(srcStride) && Aligned(mask) && Aligned(maskStride)); size_t alignedWidth = AlignLo(width, QA); size_t bodyWidth = AlignLo(width, A); v128_u8 tailMask = ShiftLeft(K8_FF, A - width + alignedWidth); v128_u8 _value = SetU8(value); *sum = 0; for (size_t row = 0; row < height; ++row) { size_t col = 0; v128_u32 sums[4] = { K32_00000000, K32_00000000, K32_00000000, K32_00000000 }; for (; col < alignedWidth; col += QA) { ConditionalSquareSum<align, compareType>(src, mask, col, _value, sums[0]); ConditionalSquareSum<align, compareType>(src, mask, col + A, _value, sums[1]); ConditionalSquareSum<align, compareType>(src, mask, col + 2 * A, _value, sums[2]); ConditionalSquareSum<align, compareType>(src, mask, col + 3 * A, _value, sums[3]); } sums[0] = vec_add(vec_add(sums[0], sums[1]), vec_add(sums[2], sums[3])); for (; col < bodyWidth; col += A) ConditionalSquareSum<align, compareType>(src, mask, col, _value, sums[0]); if (alignedWidth != width) { const v128_u8 _mask = Compare8u<compareType>(Load<false>(mask + width - A), _value); const v128_u8 _src = vec_and(vec_and(Load<false>(src + width - A), _mask), tailMask); sums[0] = vec_msum(_src, _src, sums[0]); } *sum += ExtractSum(sums[0]); src += srcStride; mask += maskStride; } }