/** set windows for trajectory */
void trajectory_set_windows(struct trajectory *traj, double d_win,
double a_win_deg, double a_start_deg)
{
traj->d_win = d_win ;
traj->a_win_rad = RAD(a_win_deg);
traj->a_start_rad = RAD(a_start_deg);
}
float find_wall(t_env *env , int **map, float each_x, unsigned int *color)
{
float x1;
float y1;
float k_x;
float k_y;
float tmp;
x1 = env->x + 0.5;
y1 = env->y + each_x;
tmp = x1;
x1 = cos(RAD(env->angle)) * (tmp - env->x) - sin(RAD(env->angle))
* (y1 - env->y);
y1 = sin(RAD(env->angle)) * (tmp - env->x) + cos(RAD(env->angle))
* (y1 - env->y);
k_x = find_x(env, map, x1, y1);
k_y = find_y(env, map, x1, y1);
*color = 0;
(x1 > 0) ? (*color = FIRST_COLOR) : (*color = SECOND_COLOR);
(k_x > k_y) ? ((y1 > 0) ? (*color = THIRD_COLOR)
: (*color = FOURTH_COLOR)) : (0);
if (k_x > k_y)
return (k_y);
return (k_x);
}
void camera_t::focus(real_t newFovX, real_t newFovY)
{
fov[X] = RAD(newFovX);
fov[Y] = RAD(newFovY);
filmSize[X] = static_cast<real_t>( frustum[nearPlane].dist * tan( fov[X]/2 ) * 2 );
filmSize[Y] = static_cast<real_t>( frustum[nearPlane].dist * tan( fov[Y]/2 ) * 2 );
frustum[topPlane].normal = forward;
frustum[topPlane].normal.rotateAround( right, fov[Y]/2 );
frustum[topPlane].dist = - ( position dot frustum[topPlane].normal );
frustum[bottomPlane].normal = forward;
frustum[bottomPlane].normal.rotateAround( right, -fov[Y]/2 );
frustum[bottomPlane].dist = - ( position dot frustum[bottomPlane].normal );
frustum[leftPlane].normal = forward;
frustum[leftPlane].normal.rotateAround( up, fov[X]/2 );
frustum[leftPlane].dist = - ( position dot frustum[leftPlane].normal );
frustum[rightPlane].normal = forward;
frustum[rightPlane].normal.rotateAround( up, -fov[X]/2 );
frustum[rightPlane].dist = - ( position dot frustum[rightPlane].normal );
frustum[nearPlane].normal = forward;
frustum[nearPlane].dist = 1;
frustum[farPlane].normal = -forward;
frustum[farPlane].dist = 1000;
}
vec3 & rotate(vec3 angle)
{
vec3 radangle = { RAD(angle.x), RAD(angle.y), RAD(angle.z) };
vec3 tmpsin = { sin(radangle.x), sin(radangle.y), sin(radangle.z) };
vec3 tmpcos = { cos(radangle.x), cos(radangle.y), cos(radangle.z) };
return rotateByCache(tmpsin, tmpcos);
}
int cPositioner::CalcHourAngle(int Longitude)
{
double Alpha = RAD(Longitude - Setup.SiteLon);
double Lat = RAD(Setup.SiteLat);
int Sign = Setup.SiteLat >= 0 ? -1 : 1; // angles to the right are positive, angles to the left are negative
return Sign * round(DEG(atan2(sin(Alpha), cos(Alpha) - cos(Lat) * SAT_EARTH_RATIO)));
}
double calc_delta_cone(t_object *object, double *vect, t_cam *cam)
{
double a;
double b;
double c;
double delta;
t_point *point;
t_point *point2;
double r;
point = init_point(vect[X], vect[Y], vect[Z], 0);
point2 = init_point(cam->x, cam->y, cam->z, 0);
point2 = inv_rotate(point2, (t_cam *) object);
point = inv_rotate(point, (t_cam *) object);
r = (double) object->r;
a = (pow(point->x, 2) + pow(point->z, 2) - (pow(point->y, 2) * pow(tan(RAD(r)), 2)));
b = 2 * ((point->x * (point2->x - object->x)) + (point->z * (point2->z - object->z))
- (point->y * (point2->y - object->y) * pow(tan(RAD(r)), 2)));
c = pow((point2->x - object->x), 2) + pow((point2->z - object->z), 2)
- (pow(((point2->y - object->y)), 2) * pow(tan(RAD(r)), 2));
delta = (b * b) - (4.0 * a * c);
vect[X] = b;
vect[Y] = a;
return (delta);
}
int star_parse_hip(struct star *s, FILE *fp)
{
char buf[STAR_HIP_RECLEN + 1];
/* Constants for use in computing galactic coordinates. */
const double c1 = PI * 282.25 / 180.0;
const double c2 = PI * 62.6 / 180.0;
const double c3 = PI * 33.0 / 180.0;
/* Read a single line from the given file. */
if (fgets(buf, STAR_HIP_RECLEN + 1, fp))
{
double ra;
double de;
double mag;
double plx;
/* Attempt to parse necessary data from the line. */
if (sscanf(buf + 51, "%lf", &ra) == 1 &&
sscanf(buf + 64, "%lf", &de) == 1 &&
sscanf(buf + 41, "%lf", &mag) == 1 &&
sscanf(buf + 79, "%lf", &plx) == 1 && plx > 0.0)
{
double b, l, n1, n2, n3;
/* Compute equatorial position in parsecs and radians. */
plx = 1000.0 / fabs(plx);
ra = RAD(ra);
de = RAD(de);
/* Compute the position in galactic coordinates. */
n1 = cos(de) * cos(ra - c1);
n2 = sin(de) * sin(c2) + cos(de) * sin(ra - c1) * cos(c2);
n3 = sin(de) * cos(c2) - cos(de) * sin(ra - c1) * sin(c2);
l = -atan2(n1, n2) + c3;
b = asin(n3);
/* l = ra; */
/* b = de; */
s->pos[0] = (float) (sin(l) * cos(b) * plx);
s->pos[1] = (float) ( sin(b) * plx);
s->pos[2] = (float) (cos(l) * cos(b) * plx);
/* Compute the absolute magnitude and color. */
s->mag = (float) (mag - 5.0 * log(plx / 10.0) / log(10.0));
star_color(buf[435], s->col);
return 1;
}
}
return 0;
}
//sjin: add solar azimuth wrapper funcions
EXPORT int64 calculate_solar_azimuth(OBJECT *obj, double lititude, double *value)
{
static SolarAngles sa; // just for the functions
double std_time = 0.0;
double solar_time = 0.0;
short int doy = 0;
DATETIME dt;
climate *cli;
if (obj == 0 || value == 0){
//throw "climate/calc_solar: null object pointer in arguement";
return 0;
}
cli = OBJECTDATA(obj, climate);
if(gl_object_isa(obj, "climate", "climate") == 0){
//throw "climate/calc_solar: input object is not a climate object";
return 0;
}
gl_localtime(obj->clock, &dt);
std_time = (double)(dt.hour) + ((double)dt.minute)/60.0 + (dt.is_dst ? -1:0);
solar_time = sa.solar_time(std_time, doy, RAD(cli->tz_meridian), RAD(obj->longitude));
double hr_ang = -(15.0 * PI_OVER_180)*(solar_time-12.0); // morning +, afternoon -
double decl = 0.409280*sin(2.0*PI*(284+doy)/365);
double alpha = (90.0 * PI_OVER_180) - lititude + decl;
*value = acos( (sin(decl)*cos(lititude) - cos(decl)*sin(lititude)*cos(hr_ang))/cos(alpha) );
return 1;
}
void
display(void) {
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
#define RAD(x) (((x)*M_PI)/180.)
gluLookAt(-sinf(RAD(rotx))*5.5,transy,cosf(RAD(rotx))*5.5, 0.,0.,0., 0.,1.,0.);
glTranslatef(0.f, 0.f, transx*10.f);
/* floor */
glColor4f(0.f,.2f,0.f,1.f);
glBegin(GL_POLYGON);
glVertex3f(-4.0, -1.0, -4.0);
glVertex3f( 4.0, -1.0, -4.0);
glVertex3f( 4.0, -1.0, 4.0);
glVertex3f(-4.0, -1.0, 4.0);
glEnd();
glEnable(GL_COLOR_MATERIAL);
glEnable(GL_LIGHTING);
glColor3f(.1f,.1f,.1f);
glPushMatrix();
glTranslatef(-1.f, -1.+.2f, -1.5f);
glScalef(.2f,.2f, .2f);
logs();
glDisable(GL_LIGHTING);
glPopMatrix();
glPushMatrix();
glTranslatef(-1.f, -1.f+.2f, -1.5f);
calcMatrix();
draw_smoke(smoke);
glPopMatrix();
glPushMatrix();
glTranslatef(-1.f, -.25f, -1.5f);
calcMatrix();
glScalef(1.f,1.f,1.);
if (texture) {
glBindTexture(GL_TEXTURE_2D, the_texture+1);
glEnable(GL_TEXTURE_2D);
}
glColor4f(intensity, intensity, intensity, opacity);
glRotatef(rot, 0., 0., 1.);
glDepthMask(0);
glBegin(GL_POLYGON);
glTexCoord2f(0.0, 0.0); glVertex2f(-1.0, -1.0);
glTexCoord2f(1.0, 0.0); glVertex2f(1.0, -1.0);
glTexCoord2f(1.0, 1.0); glVertex2f(1.0, 1.0);
glTexCoord2f(0.0, 1.0); glVertex2f(-1.0, 1.0);
glEnd();
glDepthMask(1);
glPopMatrix();
glDisable(GL_TEXTURE_2D);
glutSwapBuffers();
}
int star_parse_tyc(struct star *s, FILE *fp)
{
char buf[STAR_TYC_RECLEN + 1];
/* Constants for use in computing galactic coordinates. */
const double c1 = PI * 282.25 / 180.0;
const double c2 = PI * 62.6 / 180.0;
const double c3 = PI * 33.0 / 180.0;
/* Read a single line from the given file. */
if (fgets(buf, STAR_TYC_RECLEN + 1, fp))
{
double ra, de;
double bt, vt;
double mag;
double plx;
int hip;
/* Attempt to parse necessary data from the line. */
if (sscanf(buf + 142, "%d", &hip) == 0 &&
sscanf(buf + 152, "%lf", &ra) == 1 &&
sscanf(buf + 165, "%lf", &de) == 1 &&
sscanf(buf + 110, "%lf", &bt) == 1 &&
sscanf(buf + 123, "%lf", &vt) == 1)
{
double b, l, n1, n2, n3;
/* Compute equatorial position in parsecs and radians. */
mag = vt - 0.090 * (bt - vt);
ra = RAD(ra);
de = RAD(de);
plx = 10.0;
/* Compute the position in galactic coordinates. */
n1 = cos(de) * cos(ra - c1);
n2 = sin(de) * sin(c2) + cos(de) * sin(ra - c1) * cos(c2);
n3 = sin(de) * cos(c2) - cos(de) * sin(ra - c1) * sin(c2);
l = -atan2(n1, n2) + c3;
b = asin(n3);
s->pos[0] = (GLfloat) (sin(l) * cos(b) * plx);
s->pos[1] = (GLfloat) ( sin(b) * plx);
s->pos[2] = (GLfloat) (cos(l) * cos(b) * plx);
s->mag = (float) mag;
star_color('K', s->col);
return 1;
}
}
return 0;
}
void RobotsMover::rotateTable(int counter, float angle_step)
{
std::vector<double> angles;
angles.resize(6);
angles = p.table_start_pose;
angles[5] = RAD(180.0) - counter * RAD(angle_step);
table_mover->moveToPose(angles);
}
int cPositioner::CalcLongitude(int HourAngle)
{
double Lat = RAD(Setup.SiteLat);
double Lon = RAD(Setup.SiteLon);
double Delta = RAD(HourAngle);
double Alpha = Delta - asin(sin(M_PI - Delta) * cos(Lat) * SAT_EARTH_RATIO);
int Sign = Setup.SiteLat >= 0 ? 1 : -1;
return NormalizeAngle(round(DEG(Lon - Sign * Alpha)));
}
void Timer(int value)
{
glutTimerFunc(estado.delayMovimento, Timer, 0);
// ... accoes do temporizador ...
if(estado.teclas.q==GL_TRUE)
modelo.tanque.angCanhao+=5;
if(estado.teclas.a==GL_TRUE)
modelo.tanque.angCanhao-=5;
if(estado.teclas.z==GL_TRUE)
modelo.tanque.angTorre-=5;
if(estado.teclas.x==GL_TRUE)
modelo.tanque.angTorre+=5;
if(estado.teclas.up==GL_TRUE){
if(modelo.tanque.velocidade<=1)
modelo.tanque.velocidade+=0.20;
if(modelo.tanque.gForceV<=1)
modelo.tanque.gForceV+=0.3;
}else{
if(modelo.tanque.velocidade>0)
modelo.tanque.velocidade-=0.05;
if(modelo.tanque.gForceV>=0)
modelo.tanque.gForceV-=0.3;
}
if(estado.teclas.down){
if(modelo.tanque.velocidade>=-1)
modelo.tanque.velocidade-=0.20;
if(modelo.tanque.gForceV>=-1)
modelo.tanque.gForceV-=0.3;
}else{
if(modelo.tanque.velocidade<0)
modelo.tanque.velocidade+=0.05;
if(modelo.tanque.gForceV>=0)
modelo.tanque.gForceV+=0.3;
}
if(estado.teclas.left){
if(modelo.tanque.direccaoRodas<=3)
modelo.tanque.direccaoRodas+=0.5;
}
if(estado.teclas.right){
if(modelo.tanque.direccaoRodas>=-3)
modelo.tanque.direccaoRodas-=0.5;
}
if(!estado.teclas.left && !estado.teclas.right){
modelo.tanque.direccaoRodas = modelo.tanque.direccaoRodas*0.9;
}
modelo.tanque.direccao+=modelo.tanque.velocidade*modelo.tanque.direccaoRodas;
modelo.tanque.x = modelo.tanque.x+modelo.tanque.velocidade*cos(RAD(modelo.tanque.direccao));
modelo.tanque.y = modelo.tanque.y+modelo.tanque.velocidade*sin(RAD(modelo.tanque.direccao));
if(estado.menuActivo || modelo.parado) // sair em caso de o jogo estar parado ou menu estar activo
return;
// redesenhar o ecra
glutPostRedisplay();
}
static Pos2 random_robot_pos2(const FieldGeometry *f) {
std::uniform_real_distribution<Float> rx(
-f->field_length / 2 + ROBOT_DIAM / 2,
f->field_length / 2 + ROBOT_DIAM / 2);
std::uniform_real_distribution<Float> ry(-f->field_width / 2 + ROBOT_DIAM / 2,
f->field_width / 2 + ROBOT_DIAM / 2);
std::uniform_real_distribution<Float> rw(-RAD(180), RAD(180));
// return std::forward(rx(gen), ry(gen), rw(gen));
return {rx(gen), ry(gen), rw(gen)};
}
int cPositioner::HorizonLongitude(ePositionerDirection Direction)
{
double Delta;
if (abs(Setup.SiteLat) <= SAT_VISIBILITY_LAT)
Delta = acos(SAT_EARTH_RATIO / cos(RAD(Setup.SiteLat)));
else
Delta = 0;
if ((Setup.SiteLat >= 0) != (Direction == pdLeft))
Delta = -Delta;
return NormalizeAngle(round(DEG(RAD(Setup.SiteLon) + Delta)));
}
// Fit
void fit(orbit_t orb,int *ia)
{
int i,n;
double a[7],da[7];
double db[7]={0.1,0.1,0.002,0.1,0.1,0.01,0.0001};
// Copy parameters
a[0]=orb.eqinc*R2D;
da[0]=da[0]*R2D;
a[1]=orb.ascn*R2D;
da[1]=da[1]*R2D;
a[2]=orb.ecc;
a[3]=orb.argp*R2D;
da[3]=da[3]*R2D;
a[4]=orb.mnan*R2D;
da[4]=da[4]*R2D;
a[5]=orb.rev;
a[6]=orb.bstar;
for (i=0;i<7;i++) {
if (ia[i]==1)
da[i]=db[i];
else
da[i]=0.0;
}
// Construct struct
// a[0]: inclination
// a[1]: RA of ascending node
// a[2]: eccentricity
// a[3]: argument of periastron
// a[4]: mean anomaly
// a[5]: revs per day
// a[6]: bstar
// Count highlighted points
for (i=0,n=0;i<d.n;i++)
if (d.p[i].flag==1)
n++;
if (n>0)
versafit(n,7,a,da,chisq,0.0,1e-7,"n");
// Return parameters
orb.eqinc=RAD(a[0]);
orb.ascn=RAD(modulo(a[1],360.0));
orb.ecc=a[2];
orb.argp=RAD(modulo(a[3],360.0));
orb.mnan=RAD(modulo(a[4],360.0));
orb.rev=a[5];
orb.bstar=a[6];
return;
}
/**
Calculate the solar radation for a surface facing the given compass point.
@param cpt compass point of the direction the surface is facing
@param doy day of year
@param lat latitude of the surface
@param sol_time the solar time of day
@param dnr Direct Normal Radiation
@param dhr Diffuse Horizontal Radiation
@param ghr Global Horizontal Radiation
@param gnd_ref Ground Reflectivity
@param vert_angle the angle of the surface relative to the horizon (Default is 90 degrees)
*/
double tmy2_reader::calc_solar(COMPASS_PTS cpt, short doy, double lat, double sol_time, double dnr, double dhr, double ghr, double gnd_ref, double vert_angle = 90)
{
SolarAngles *sa = new SolarAngles();
double surface_angle = surface_angles[cpt];
double cos_incident = sa->cos_incident(lat,RAD(vert_angle),RAD(surface_angle),sol_time,doy);
delete sa;
//double solar = (dnr * cos_incident + dhr/2 + ghr * gnd_ref);
double solar = dnr * cos_incident + dhr;
if (peak_solar==0 || solar>peak_solar) peak_solar = solar;
return solar;
}
Vector vector(Coordinate c){
Vector v;
double lat = RAD(c.latitude);
double lon = RAD(c.longitude);
v.x = cos(lat) * sin(lon);
v.y = sin(lat);
v.z = cos(lat) * cos(lon);
return v;
}
/* calculate the real time current value */
int32_t vsm_get_dr_current(vam_stastatus_t *last, vam_stastatus_t *current)
{
double deltaT = 0.0;
double v, s, dR;
double dir, lon1, lat1, lon2, lat2; /* Radians */
uint32_t t = osal_get_systemtime();
if(!last || !current)
{
return -1;
}
deltaT = ((t>=last->time) ? (t-last->time) : \
(t+RT_UINT32_MAX - last->time)) / 1000.0;
memcpy(current, last, sizeof(vam_stastatus_t));
if(deltaT == 0 || (last->speed < 5))
{
return 0;
}
/* deltaT != 0, the calculate the "current" value */
lon1 = RAD((double)last->pos.lon);
lat1 = RAD((double)last->pos.lat);
dir = RAD((double)last->dir);
/* uniform rectilinear motion */
v = last->speed / 3.6f;
s = v*deltaT;
/* lat2 = asin( sin lat1 * cos dR + cos lat1 * sin dR * cos θ )
lon2 = lon1 + atan2( sin θ * sin dR * cos lat1, cos dR- sin lat1 * sin lat2 )
where lat is latitude, lon is longitude, θis the bearing (clockwise from north),
dR is the angular distance d/R; d being the distance travelled, R the earth’s radius */
dR = s / EARTH_RADIUS / 1000.0;
lat2 = asin(sin(lat1)*cos(dR) + cos(lat1)*sin(dR)*cos(dir));
lon2 = lon1 + atan2(sin(dir)*sin(dR)*cos(lat1), cos(dR)-sin(lat1)*sin(lat2));
current->time = t;
current->dir = dir * 180.0 / PI;
current->speed = v*3.6;
current->pos.lon = lon2 * 180.0 / PI;
current->pos.lat = lat2 * 180.0 / PI;
#if 0
char buf[100];
sprintf(buf, "(lon=%f,lat=%f),h=%f,d=%f,s=%f,v=%f", current->pos.lon, current->pos.lat, current->dir,
vsm_get_relative_pos(last, current, 0), s, v);
rt_kprintf("c%d%s\r\n", t, buf);
#endif
return 0;
}
int keyHandler(void *null) {
distance = 2.0f;
while(e.type != SDL_QUIT) {
keyPress = SDL_GetKeyState(NULL);
if(keyPress[SDLK_ESCAPE]) {
SDL_Quit();
break;
}
if(keyPress[SDLK_w] || keyPress[SDLK_UP]) {
player.idlerot = player.ent.rotation;
if(player.speed < player.maxspd && SDL_GetTicks() - t2 >= 200) {
player.speed += player.accel;
t2 = SDL_GetTicks();
}
player.ent.x += player.speed * (float) sin(RAD(player.ent.rotation));
player.ent.y -= player.speed * (float) cos(RAD(player.ent.rotation));
} else if(player.speed > 0.0f) {
if(SDL_GetTicks() - t2 >= 200) {
player.speed -= (player.speed / 2) * player.decel;
t2 = SDL_GetTicks();
if(player.speed < 0.01f) {
player.speed = 0.0f;
}
}
player.ent.x += player.speed * (float) sin(RAD(player.idlerot));
player.ent.y -= player.speed * (float) cos(RAD(player.idlerot));
}
if(keyPress[SDLK_d] || keyPress[SDLK_RIGHT]) {
player.ent.rotation += distance * 1.0f;
} else if(keyPress[SDLK_a] || keyPress[SDLK_LEFT]) {
player.ent.rotation -= distance * 1.0f;
}
if(t1 == 0 || SDL_GetTicks() - t1 > 100) {
t1 = SDL_GetTicks();
if(keyPress[SDLK_SPACE]) {
bullets[nextBullet] = player.ent;
bullets[nextBullet].speed = 3 + player.maxspd;
bullets[nextBullet++].flags = FLAG_ALIVE;
if(nextBullet >= MAX_BULLETS) nextBullet = 0;
}
}
if(player.ent.rotation > 359) player.ent.rotation = 0;
else if(player.ent.rotation < 0) player.ent.rotation = 359;
//free(keyPress);
SDL_Delay(10);
}
return 0;
}
void b3n_draw_circleofcircles(int x, int y, unsigned int size, float angle, int32_t* colors, unsigned int nbr, s_b3n_env* env)
{
float offset;
unsigned int i;
if (nbr < 3)
return ;
offset = 360.f / nbr;
i = 0;
while (i < nbr)
{
b3n_draw_circle(x + (size * cos(RAD(angle + (i * offset)))), y + (size * sin(RAD(angle + (i * offset)))), (size * M_PI) / nbr, colors[i], env);
i++;
}
}
static void ChangeViewpoint( filter_t *p_filter, const vlc_viewpoint_t *p_vp)
{
filter_sys_t *p_sys = (filter_sys_t *)p_filter->p_sys;
#define RAD(d) ((float) ((d) * M_PI / 180.f))
p_sys->f_teta = -RAD(p_vp->yaw);
p_sys->f_phi = RAD(p_vp->pitch);
p_sys->f_roll = RAD(p_vp->roll);
if (p_vp->fov >= FIELD_OF_VIEW_DEGREES_DEFAULT)
p_sys->f_zoom = 0.f; // no unzoom as it does not really make sense.
else
p_sys->f_zoom = (FIELD_OF_VIEW_DEGREES_DEFAULT - p_vp->fov) / (FIELD_OF_VIEW_DEGREES_DEFAULT - FIELD_OF_VIEW_DEGREES_MIN);
#undef RAD
}
void paint() {
static const int ox = 150;
static const int oy = 150;
static const int hl = 46;
static const int ml = 74;
static const int sl = 120;
int i;
beginPaint();
clearDevice();
// circle
setPenWidth(2);
setPenColor(BLACK);
setBrushColor(WHITE);
ellipse(25, 25, 275, 275);
// label
setPenWidth(1);
setPenColor(BLACK);
for (i = 0; i < 12; ++i) {
moveTo(ox + 115 * sin(RAD(180 - i * 30)), oy + 115 * cos(RAD(180 - i * 30)));
lineTo(ox + 125 * sin(RAD(180 - i * 30)), oy + 125 * cos(RAD(180 - i * 30)));
}
// hour
setPenWidth(8);
setPenColor(BLACK);
moveTo(ox, oy);
lineTo(ox + hl * sin(RAD(180 - h * 30)), oy + hl * cos(RAD(180 - h * 30)));
// minute
setPenWidth(4);
setPenColor(GREEN);
moveTo(ox, oy);
lineTo(ox + ml * sin(RAD(180 - m * 6)), oy + ml * cos(RAD(180 - m * 6)));
// second
setPenWidth(2);
setPenColor(RED);
moveTo(ox, oy);
lineTo(ox + sl * sin(RAD(180 - s * 6)), oy + sl * cos(RAD(180 - s * 6)));
endPaint();
}
void VectorLocalization2D::lowVarianceResample()
{
vector<Particle2D> newParticles;
newParticles.resize(numParticles);
float totalWeight = 0.0;
float newWeight = 1.0/float(numParticles);
int numRefinedParticles = (int) particlesRefined.size();
refinedImportanceWeights = unrefinedImportanceWeights = 0.0;
for(int i=0; i<numRefinedParticles; i++){
//Get rid of particles with undefined weights
if(isnan(particlesRefined[i].weight) || isinf(particlesRefined[i].weight) || particlesRefined[i].weight<0.0)
particlesRefined[i].weight = 0.0;
totalWeight += particlesRefined[i].weight;
if(i<numParticles)
refinedImportanceWeights += particlesRefined[i].weight;
else
unrefinedImportanceWeights += particlesRefined[i].weight;
}
if(totalWeight<FLT_MIN){
//TerminalWarning("Particles have zero total weight!");
for(int i=0; i<numParticles; i++){
particles[i].weight = newWeight;
}
return;
//exit(0);
}
float weightIncrement = totalWeight/float(numParticles);
if(weightIncrement<FLT_MIN) TerminalWarning("Particle weights less than float precision");
numRefinedParticlesSampled = numUnrefinedParticlesSampled = 0;
float x = frand(0.0f,totalWeight);
int j=0;
float f=particlesRefined[0].weight;
for(int i=0; i<numParticles; i++){
while(f<x){
j = (j+1)%numRefinedParticles;
f += particlesRefined[j].weight;
}
if(j<numParticles)
numRefinedParticlesSampled++;
else
numUnrefinedParticlesSampled++;
newParticles[i] = particlesRefined[j];
newParticles[i].weight = newWeight;
if(particlesRefined[i].weight < FLT_MIN){
//This particle was depleted: add replacement noise
vector2f deltaLoc = vector2f(frand(-1.0,1.0),frand(-1.0,1.0))*0.05;
float deltaAngle = frand(-1.0,1.0)*RAD(5.0);
newParticles[i].loc += deltaLoc;
newParticles[i].angle += deltaAngle;
}
x += weightIncrement;
}
particles = newParticles;
}
t_pos cone_normal(t_obj *o, t_pos r, t_pos p)
{
t_pos n;
t_pos a;
t_pos c;
t_data_cone *data;
(void)r;
(void)p;
n = get_pos(0, 0, 0);
data = (t_data_cone*)o->data;
if (data)
{
p = transform(o->o, o->sp, p, o->rot);
c = get_pos(0, data->top, 0);
a = get_pos(0, 1, 0);
if (p.y < data->top)
a = get_pos(0, -1, 0);
n = p;
pos_mult_to_number(&a, pos_norme(pos_vector(c, p))
/ cos(RAD(data->ang / 2)));
pos_add_to_pos(&c, a);
pos_mult_to_number(&c, -1);
pos_add_to_pos(&n, c);
n = pos_normalize(n);
}
return (n);
}
void rotateBy(float axis[3], float phi) {
GLfloat m[4][4];
float q[4] = {0};
axis_to_quat(axis, RAD(phi), q);
build_rotmatrix(m, q);
glMultMatrixf(&m[0][0]);
}
GLvoid Rotate16fv(GLfloat * m, GLfloat a, GLint ort)
{
a = RAD(a);
GLuint i;
GLfloat tmp;
switch (ort) {
case ORT_X:
for (i = 1; i < 12; i += 4) {
tmp = GLfloat ( m[i] * cos(a) - m[i + 1] * sin(a));
m[i + 1] = GLfloat ( m[i] * sin(a) + m[i + 1] * cos(a));
m[i] = tmp;
}
break;
case ORT_Y:
for (i = 0; i < 12; i += 4) {
tmp = GLfloat (m[i] * cos(a) + m[i + 2] * sin(a));
m[i + 2] = GLfloat ( -m[i] * sin(a) + m[i + 2] * cos(a));
m[i] = tmp;
}
break;
case ORT_Z:
for (i = 0; i < 12; i += 4) {
tmp = GLfloat (m[i] * cos(a) - m[i + 1] * sin(a));
m[i + 1] = GLfloat (m[i] * sin(a) + m[i + 1] * cos(a));
m[i] = tmp;
}
break;
}
}
static void scene_video_render(void *data)
{
struct obs_scene *scene = data;
struct obs_scene_item *item;
pthread_mutex_lock(&scene->mutex);
item = scene->first_item;
while (item) {
if (obs_source_removed(item->source)) {
struct obs_scene_item *del_item = item;
item = item->next;
obs_sceneitem_remove(del_item);
continue;
}
gs_matrix_push();
gs_matrix_translate3f(item->origin.x, item->origin.y, 0.0f);
gs_matrix_scale3f(item->scale.x, item->scale.y, 1.0f);
gs_matrix_rotaa4f(0.0f, 0.0f, 1.0f, RAD(-item->rot));
gs_matrix_translate3f(-item->pos.x, -item->pos.y, 0.0f);
obs_source_video_render(item->source);
gs_matrix_pop();
item = item->next;
}
pthread_mutex_unlock(&scene->mutex);
}
void CPageImageProcess::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pScrollBar)
{
// TODO: 在此添加消息处理程序代码和/或调用默认值
CSliderCtrl *pSlider = (CSliderCtrl*)pScrollBar;
CMainFrame *pFrame = (CMainFrame *)AfxGetMainWnd();
CCOXRayView *pView = (CCOXRayView *)pFrame->GetActiveView();
//多个 SLIDER 控件控制
switch( pSlider->GetDlgCtrlID() )
{
case IDC_SLIDER_GAMMA:
{
m_dbEditGamma = tan(RAD(pSlider->GetPos()));
m_Gamma.SetGamma(m_dbEditGamma);
if (m_bLBDown)
{
::SendMessage(pView->GetSafeHwnd(),WM_GAMMA_CHANGE_PREVIEW,0,0);
}
}
break;
}
CDialogEx::OnHScroll(nSBCode, nPos, pScrollBar);
}
void display(void)
{
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
//glFrustum (-2.0, 2.0, -2.0, 2.0, 1.0, 20.0);
glOrtho (-2.0, 2.0, -2.0, 2.0, 0.0, 20.0);
glMatrixMode (GL_MODELVIEW);
glLoadIdentity();
gluLookAt (0.0, 0.0, g_radius, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
glScalef (1.0, 1.0, 1.0);
glRotatef(g_phi, 0.0, 1.0, 0.0);
glRotatef(g_theta, cos(RAD(g_phi)), 0.0, -sin(RAD(g_phi)));
//glPolygonMode (GL_FRONT_AND_BACK, GL_LINE);
//glEnable(GL_MULTISAMPLE);
//glEnable(GL_POLYGON_SMOOTH);
//glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
glEnable(GL_DEPTH_TEST);
glColor3f (1.0, 0.0, 0.0);
glBegin(GL_TRIANGLE_STRIP);
glVertex3f(1, 1, 0);
glVertex3f(1, -1, 0);
glVertex3f(-1, 1, 0);
glVertex3f(-1, -1, 0);
glEnd();
glColor3f (0.0, 1.0, 0.0);
glBegin(GL_TRIANGLE_STRIP);
glVertex3f(0.0, 1, 1);
glVertex3f(0.0, 1, -1);
glVertex3f(0.0, -1, 1);
glVertex3f(0.0, -1, -1);
glEnd();
glColor3f (0.0, 0.0, 1.0);
glBegin(GL_TRIANGLE_STRIP);
glVertex3f(1, 0, 1);
glVertex3f(1, 0, -1);
glVertex3f(-1, 0, 1);
glVertex3f(-1, 0, -1);
glEnd();
glDisable(GL_DEPTH_TEST);
glutSwapBuffers();
}
