int pan()
{
int flag = 0;
int i=1;
for(;i<3;i++)
{
if(near(DIS(a[i-1],a[i]),DIS(a[i],a[i+1])) == 0)
{
if(formZ(a[i-1],a[i],a[i+1])==0)return 0;
// printf(" %d %d %d %d %lf %lf\n",a[i-1],a[i],a[i],a[i+1],DIS(a[i-1],a[i]),DIS(a[i],a[i+1]));
return 0;
}
}
if(i == 3)
{
if(near(DIS(a[i-1],a[i]),DIS(a[i],a[0])) == 0)
{
// printf(" 2: %d %d %d %d %lf %lf\n",a[i-1],a[i],a[i],a[i+1],DIS(a[i-1],a[i]),DIS(a[i],a[i+1]));
return 0;
}
}
if(near(DIS(a[0],a[2]),DIS(a[0],a[1])*gen2) == 0)
{
// printf(" 3 : %d %d %d %d %lf %lf\n",a[i-1],a[i],a[i],a[i+1],DIS(a[i-1],a[i]),DIS(a[i],a[i+1]));
return 0;
}
if(near(DIS(a[1],a[3]),DIS(a[0],a[1])*gen2) == 0)
{
// printf("4: %d %d %d %d %lf %lf\n",a[i-1],a[i],a[i],a[i+1],DIS(a[i-1],a[i]),DIS(a[i],a[i+1]));
return 0;
}
return 1;
}
void trimxy(FILE *output, FILE *input) {
int inflag=0;
int xold=0, yold=0, xnew=0, ynew=0;
int xa, ya, xb, yb, xc, yc;
while (fgets(line, sizeof(line), input) != NULL) {
fputs(line, output);
if (*line == ']') break;
}
while (fgets(line, sizeof(line), input) != NULL) {
if (*line == ']' || *line == '\n' || *line == '%' || *line == 'h' ||
strchr(line, '%') != NULL) {
fputs(line, output);
inflag = 0;
}
else {
if (strstr(line, " m") != NULL) {
sscanf(line, "%d %d", &xnew, &ynew);
fputs(line, output);
}
else if (strstr(line, " l") != NULL) {
sscanf(line, "%d %d", &xnew, &ynew);
if (!near(xnew, ynew, xold, yold)) fputs(line, output);
}
else if (strstr(line, " c") != NULL) {
sscanf(line, "%d %d %d %d %d %d", &xa, &ya, &xb, &yb, &xc, &yc);
xnew = xc; ynew = yc;
if (!near(xnew, ynew, xold, yold)) fputs(line, output);
}
inflag = 1;
xold = xnew; yold = ynew;
}
}
}
// @param[in] isStereo Whether scene is in stereo (widens near/far planes to fit both eyes)
void Lens::frustum(Frustumd& f, const Pose& p, double aspect) const {//, bool isStereo) const {
Vec3d ur, uu, uf;
p.directionVectors(ur, uu, uf);
const Vec3d& pos = p.pos();
double nh = heightAtDepth(near());
double fh = heightAtDepth(far());
double nw = nh * aspect;
double fw = fh * aspect;
// // This effectively creates a union between the near/far planes of the
// // left and right eyes. The offsets are computed by using the law
// // of similar triangles.
// if(isStereo){
// nw += fabs(0.5*eyeSep()*(focalLength()-near())/focalLength());
// fw += fabs(0.5*eyeSep()*(focalLength()- far())/focalLength());
// }
Vec3d nc = pos + uf * near(); // center point of near plane
Vec3d fc = pos + uf * far(); // center point of far plane
f.ntl = nc + uu * nh - ur * nw;
f.ntr = nc + uu * nh + ur * nw;
f.nbl = nc - uu * nh - ur * nw;
f.nbr = nc - uu * nh + ur * nw;
f.ftl = fc + uu * fh - ur * fw;
f.ftr = fc + uu * fh + ur * fw;
f.fbl = fc - uu * fh - ur * fw;
f.fbr = fc - uu * fh + ur * fw;
f.computePlanes();
}
void Pick3DObjectExample::CreateWorldSpaceRayFromScreen(const Eegeo::v2& screenPoint, Ray& ray)
{
const Eegeo::Camera::RenderCamera& renderCamera = m_cameraProvider.GetRenderCamera();
//normalize the point
double nx = 2.0 * screenPoint.GetX() / m_renderContext.GetScreenWidth() - 1;
double ny = - 2.0 * screenPoint.GetY() / m_renderContext.GetScreenHeight() + 1;
//prepare near and far points
Eegeo::v4 near(nx, ny, 0.0f, 1.0);
Eegeo::v4 far(nx, ny, 1.0f, 1.0);
Eegeo::m44 invVP;
Eegeo::m44::Inverse(invVP, renderCamera.GetViewProjectionMatrix());
//unproject the points
Eegeo::v4 unprojectedNear = Eegeo::v4::Mul(near, invVP);
Eegeo::v4 unprojectedFar = Eegeo::v4::Mul(far, invVP);
//convert to 3d
Eegeo::v3 unprojectedNearWorld = unprojectedNear / unprojectedNear.GetW();
Eegeo::v3 unprojectedFarWorld = unprojectedFar / unprojectedFar.GetW();
//check intersection with a ray cast from camera position
ray.m_origin = renderCamera.GetEcefLocation();
ray.m_direction = (unprojectedNearWorld - unprojectedFarWorld).Norm();
}
void DepthCamera::init_float_tex(int w, int h)
{
if ( ftex.isAllocated() )
return;
ftex.allocate( w, h, GL_LUMINANCE32F_ARB );
fpix.allocate( w, h, 1 );
fpix.set( 0 );
int near_mm = (int)(near() * 1000);
int far_mm = (int)(far() * 1000);
flut = new float[ far_mm ];
flut[0] = 0;
for ( int i = 1; i < far_mm; i++ )
{
/*
* WARNING
* this interpolation is related
* to z_norm_to_mts in render.vert shader
*/
flut[ i ] = ofMap( i,
near_mm, far_mm,
1., 0., true );
}
};
bool Unproject(real x, real y, TGVectorF4 &result)
{
y = wHeight - y;
TGMatrix4 A = camera.GetProjection() * camera.GetView();
if(!A.Invert())
{
Debug("Invert failed");
return false;
}
real normx = 2.*x/wWidth - 1.;
real normy = 2.*y/wHeight - 1.;
//Debug("x: %g; y: %g", normx, normy);
TGVectorF4 far(normx, normy, 1);
far = A*far;
if(far.W == 0)
return false;
far.W = 1./far.W;
far *= far.W;
TGVectorF4 near(normx, normy, 0);
near = A*near;
if(near.W == 0)
return false;
near.W = 1./near.W;
near *= near.W;
TGVectorF4 dir = far - near;
real t = -near.Y / dir.Y;
result = near + dir*t;
return true;
}
int formZ(int a,int b,int c)
{
double xx0 = x[a]-x[b];
double xx1 = x[c]-x[b];
double yy0 = y[a]-y[b];
double yy1 = y[c]-y[b];
if(near(dian(xx0,xx1,yy0,yy1),0.0)==1)
return 1;
return 0;
}
//----------------------------------------------------------------------------------------------------------------------
ngl::Vec3 NGLDraw::getWorldSpace(const int _x, const int _y)
{
ngl::Mat4 m;
m = m*m_mouseGlobalTX;
ngl::Mat4 t=m_cam->getProjectionMatrix();
ngl::Mat4 v=m_cam->getViewMatrix();
// as ngl:: and OpenGL use different formats need to transpose the matrix.
t.transpose();
v.transpose();
m.transpose();
ngl::Mat4 inverse=(t*v*m).inverse();
ngl::Vec4 tmp(0,0,-1.0f,1.0f);
// convert into NDC
tmp.m_x=(2.0f * _x) / m_width- 1.0f;
tmp.m_y=1.0f - (2.0f * _y) / m_height;
// scale by inverse MV * Project transform
ngl::Vec4 near(tmp.m_x,tmp.m_y,-1.0f,1.0f);
ngl::Vec4 far(tmp.m_x,tmp.m_y,1.0f,1.0f);
//get world point on near and far clipping planes
ngl::Vec4 obj_near=inverse*near;
ngl::Vec4 obj_far=inverse*far;
// Scale by w
obj_near/=obj_near.m_w;
obj_far/=obj_far.m_w;
ngl::Vec3 nearPoint(obj_near.m_x,obj_near.m_y,obj_near.m_z);
ngl::Vec3 farPoint(obj_far.m_x,obj_far.m_y,obj_far.m_z);
//create ray
ngl::Vec3 rayDir(farPoint-nearPoint);
if(rayDir.lengthSquared() == 0.0f)
{
std::cout<<"Ray Direction in getWorldSpace equals zero, can't normalise"<<std::endl;
}
else
{
rayDir.normalize();
}
//calculate distance to zx plane
float dist = (-nearPoint.m_y)/rayDir.m_y;
//set world space coordinate where y = 0
ngl::Vec3 obj(nearPoint.m_x + (dist*rayDir.m_x),nearPoint.m_y + (dist*rayDir.m_y),nearPoint.m_z + (dist*rayDir.m_z));
obj.m_y = 0.0;
return obj;
}
void rotate(Uint8 id)
{
if (obj[id].AI)
{
if (!obj[id].jump.power || (obj[id].state == STATE_FALL &&
!(obj[id].AI >= 2 && near(255, id, 50))))
obj[id].dir = (obj[id].dir==DIR_LEFT)*2;
if (obj[id].jump.power && obj[id].state != STATE_FALL &&
!obj[id].jump.frame) obj[id].jump.frame = 1;
}
}
/// Setup perspective camera in OpenGL
void Camera::setup(qreal _fov, qreal _aspect)
{
QMatrix4x4 _m;
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
// perspective projection
_m.perspective(_fov, _aspect, near(), far());
_m.lookAt(eye(),center(),up());
// Apply matrix to OpenGL
glMultMatrixf(_m.constData());
}
void fps_calc(FRAMESPS *fps) {
if(!fps->fps_expected) {
fps->fps_expected = FPS;
}
if(SDL_GetTicks() > fps->fpstime+1000) {
fps->fps_show = fps->fps;
fps->fps = 0;
fps->fpstime = SDL_GetTicks();
} else {
fps->fps++;
}
fps->fps_expected = near(); //anything else for that? should contain fps->fps_expected, fps->fps_show, 0.1
}
/* void Dstar::updateGoal(int x, int y)
* --------------------------
* This is somewhat of a hack, to change the position of the goal we
* first save all of the non-empty on the map, clear the map, move the
* goal, and re-add all of non-empty cells. Since most of these cells
* are not between the start and goal this does not seem to hurt
* performance too much. Also it free's up a good deal of memory we
* likely no longer use.
*/
void Dstar::updateGoal(int x, int y) {
list< pair<ipoint2, double> > toAdd;
pair<ipoint2, double> tp;
ds_ch::iterator i;
list< pair<ipoint2, double> >::iterator kk;
for(i=cellHash.begin(); i!=cellHash.end(); i++) {
if (!near(i->second.cost, C1)) {
tp.first.x = i->first.x;
tp.first.y = i->first.y;
tp.second = i->second.cost;
toAdd.push_back(tp);
}
}
cellHash.clear();
openHash.clear();
while(!openList.empty())
openList.pop();
k_m = 0;
s_goal.x = x;
s_goal.y = y;
cellInfo tmp;
tmp.g = tmp.rhs = 0;
tmp.cost = C1;
cellHash[s_goal] = tmp;
insert(s_goal);
tmp.g = tmp.rhs = heuristic(s_start,s_goal);
tmp.cost = C1;
cellHash[s_start] = tmp;
s_start = calculateKey(s_start);
s_last = s_start;
for (kk=toAdd.begin(); kk != toAdd.end(); kk++) {
updateCell(kk->first.x, kk->first.y, kk->second);
}
}
void Vertex::refreshShadowPosition(Light& light, double planeY)
{
// [x, y, (-Lx * x - Ly * y - Lw) / Lz]
float x = light.position.data[Vector4::X];
float y = light.position.data[Vector4::Y];
float z = light.position.data[Vector4::Z];
float w = light.position.data[Vector4::W];
shadowPosition.setData(
position.data[Vector3::X],
position.data[Vector3::Y] - planeY,
( x * position.data[Vector3::X]
+ y * position.data[Vector3::Y]
- w
) / (near(z, 0) ? 0.0001 : z)
);
}
/* void Dstar::updateCell(int x, int y, double val)
* --------------------------
* As per [S. Koenig, 2002]
*/
void Dstar::updateCell(int x, int y, double val) {
state u;
u.x = x;
u.y = y;
if ((u == s_start) || (u == s_goal)) return;
// if the value is still the same, don't need to do anything
ds_ch::iterator cur = cellHash.find(u);
if ((cur != cellHash.end()) && (near(cur->second.cost,val))) return;
makeNewCell(u);
cellHash[u].cost = val;
updateVertex(u);
}
Ray mousePosition(){
int x, y, width, height;
glfwGetMousePos(&x, &y);
glfwGetWindowSize(&width, &height);
y = height-y;
GLdouble modelview[16];
GLdouble projection[16];
GLint viewport[4];
glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
glGetDoublev(GL_PROJECTION_MATRIX, projection);
glGetIntegerv(GL_VIEWPORT, viewport);
double nx, ny, nz;
double fx, fy, fz;
gluUnProject( x, y, -1, modelview, projection, viewport, &nx, &ny, &nz);
gluUnProject( x, y, 1, modelview, projection, viewport, &fx, &fy, &fz);
V3f near((float)nx, (float)ny, (float)nz);
V3f far((float)fx, (float)fy, (float)fz);
V3f dir(far); dir -= near;
/* //we need to step maximum integer steps, to get into evry layer.
int steps = (int)MAX3(ABS(near.x-far.x),ABS(near.y-far.y),ABS(near.z-far.z));
V3f step(dir); step /= steps;
V3f cur(near); //start with near;
for(int i = 0; i < steps; i++){
cur += step;
if((cur.x > 0) && (cur.y > 0) && (cur.z > 0) &&
(cur.x < 256) && (cur.y < 256) && (cur.z < 255)){
//inside the cube, let's check.
int offset = vol.getOffset((int)cur.x, (int)cur.y, (int)cur.z);
if(vol.vol[offset] > tw_cursor_hit){ // hit condition tuned by gui
set_cursor(cur+step*tw_cursor_depth); // how deep below the surface we want our cursor to go.
//say("found",cursor)
return;
};
};
};
};*/
return Ray(near, dir);
};
void playerChargeSpecialAttack(App *app, Player *player){
Body *body = &player->body;
Body *head_body = &app->game.head.body;
if(!player->grabbing && near(body, head_body) && player->special_attack > 90 && !app->game.winner && app->game.head.body.life > 0){
float a = body->angle * M_PI / 180;
float dx = cos(a) * HOLD_DISTANCE;
float dy = sin(a) * HOLD_DISTANCE;
float tx = head_body->pos.x - dx;
float ty = head_body->pos.y + dy;
if(is_empty(&app->game, body, (int)tx,(int)body->pos.y))
body->pos.x = tx;
if(is_empty(&app->game, body, (int)body->pos.x,(int)ty))
body->pos.y = ty;
player->special_attack = 0;
player->grabbing = 1;
head_body->action = ACTION_ATTACK;
}
if(player->grabbing) {
float dist = sqrt(
pow(app->game.head.body.pos.x - player->door.x, 2)+
pow(app->game.head.body.pos.y - player->door.y, 2)
);
if(dist < tileSize*.45){
app->game.winner = player;
player->grabbing = 0;
}
}
if(!player->grabbing && player->special_attack < 100 && app->game.head.body.life > 0) {
player->special_attack += 3;
player->power_body.action = ACTION_MOVE;
}
}
/* void Dstar::updateVertex(state u)
* --------------------------
* As per [S. Koenig, 2002]
*/
void Dstar::updateVertex(state &u) {
list<state> s;
list<state>::iterator i;
if (u != s_goal) {
getSucc(u,s);
double tmp = INFINITY;
double tmp2;
for (i=s.begin();i != s.end(); i++) {
tmp2 = getG(*i) + cost(u,*i);
if (tmp2 < tmp) tmp = tmp2;
}
//if (!near(getRHS(u),tmp))
setRHS(u,tmp);
}
if (!near(getG(u),getRHS(u))) insert(u);
else remove(u);
}
void KDLKinematicsPlugin::getRandomConfiguration(const KDL::JntArray &seed_state,
const std::vector<double> &consistency_limits,
KDL::JntArray &jnt_array,
bool lock_redundancy) const
{
std::vector<double> values(dimension_, 0.0);
std::vector<double> near(dimension_, 0.0);
for (std::size_t i = 0 ; i < dimension_; ++i)
near[i] = seed_state(i);
// Need to resize the consistency limits to remove mimic joints
std::vector<double> consistency_limits_mimic;
for(std::size_t i = 0; i < dimension_; ++i)
{
if(!mimic_joints_[i].active)
continue;
consistency_limits_mimic.push_back(consistency_limits[i]);
}
joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near, consistency_limits_mimic);
for (std::size_t i = 0; i < dimension_; ++i)
{
bool skip = false;
if (lock_redundancy)
for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
if (redundant_joint_indices_[j] == i)
{
skip = true;
break;
}
if (skip)
continue;
jnt_array(i) = values[i];
}
}
Ray Camera::GetScreenRay(float x, float y) const
{
Ray ret;
// If projection is invalid, just return a ray pointing forward
if (!IsProjectionValid())
{
ret.origin_ = node_ ? node_->GetWorldPosition() : Vector3::ZERO;
ret.direction_ = node_ ? node_->GetWorldDirection() : Vector3::FORWARD;
return ret;
}
Matrix4 viewProjInverse = (GetProjection(false) * GetView()).Inverse();
// The parameters range from 0.0 to 1.0. Expand to normalized device coordinates (-1.0 to 1.0) & flip Y axis
x = 2.0f * x - 1.0f;
y = 1.0f - 2.0f * y;
Vector3 near(x, y, 0.0f);
Vector3 far(x, y, 1.0f);
ret.origin_ = viewProjInverse * near;
ret.direction_ = ((viewProjInverse * far) - ret.origin_).Normalized();
return ret;
}
char isab(uchar *s1,uchar *s2) {
r0 = (char)memcmp(s1,s2,near(s1,s2));
return(r0 ? r0>0 : s1>s2);
}
bool BoundBox::collidesZ(BoundBox& other){
return !(far() > other.near() || near() < other.far());
}
u8
same_line(const vertex& a, const vertex& b, const vertex& c)
{
return near((b.y - a.y) * (c.x - b.x), (c.y - b.y) * (b.x - a.x), 0.00001);
}
static void update_ants(antmazestruct *mp)
{
int i;
GLfloat df[4];
df[0] = df[1] = df[2] = 0.8*mp->fadeout;
df[3] = 1.0;
/* fade out */
if(mp->fadeoutspeed < -0.00001) {
if(mp->fadeout <= 0.0) {
/* switch boards: rebuild old board, increment current */
mp->currentboard = (mp->currentboard+1)%BOARDCOUNT;
build_board(mp, mp->currentboard);
mp->fadeoutspeed = 0.02;
}
mp->fadeout += mp->fadeoutspeed;
glLightfv(GL_LIGHT0, GL_DIFFUSE, df);
glLightfv(GL_LIGHT1, GL_DIFFUSE, df);
}
/* fade in */
if(mp->fadeoutspeed > 0.0001) {
mp->fadeout += mp->fadeoutspeed;
if(mp->fadeout >= 1.0) {
mp->fadeout = 1.0;
mp->fadeoutspeed = 0.0;
mp->entroducing = 12;
}
glLightfv(GL_LIGHT0, GL_DIFFUSE, df);
glLightfv(GL_LIGHT1, GL_DIFFUSE, df);
}
for(i = 0; i < ANTCOUNT; ++i) {
if(!mp->anton[i] && mp->elevator < 1.0) {
/* turn on ant */
if(mp->entroducing > 0 && mp->introduced <= 0 && random()%100 == 0) {
mp->anton[i] = 1;
mp->part[i] = 0;
mp->antsize[i] = 0.0;
mp->antposition[i][0] = -4.0;
mp->antposition[i][1] = 5.0;
mp->antdirection[i] = PI/2.0;
mp->bposition[i][0] = 0;
mp->bposition[i][1] = 8;
mp->introduced = 300;
mp->entroducing--;
}
continue;
}
if(mp->part[i] == 0 && mp->antsize[i] < 1.0) {
mp->antsize[i] += 0.02;
continue;
}
if(mp->part[i] > mp->antpathlength[i] && mp->antsize[i] > 0.0) {
mp->antsize[i] -= 0.02;
if(mp->antvelocity[i] > 0.0) {
mp->antvelocity[i] -= 0.02;
}
else { mp->antvelocity[i] = 0.0; }
continue;
}
if(mp->part[i] > mp->antpathlength[i] && mp->antsize[i] <= 0.0) {
mp->antvelocity[i] = 0.02;
/* if(i != 0) { */
antmaterial[i] = materials[random()%MATERIALS];
/* } */
mp->antdirection[i] = PI/2.0;
mp->bposition[i][0] = 0;
mp->bposition[i][1] = 8;
mp->part[i] = 0;
mp->antsize[i] = 0.0;
mp->anton[i] = 0;
mp->antposition[i][0] = -4.0;
mp->antposition[i][1] = 5.0;
/* /\* reset camera *\/ */
/* if(i == focus) { */
/* started = 0; */
/* ant_step = 0.0; */
/* } */
/* check for the end */
if(mp->entroducing <= 0) {
int ao = 0, z = 0;
for(z = 0; z < ANTCOUNT; ++z) {
if(mp->anton[z]) { ao = 1; break; }
}
if(ao == 0) {
mp->fadeoutspeed = -0.02;
}
}
}
/* near goal, bend path towards next step */
if(near(mp->antposition[i], mp->antpath[i][mp->part[i]])) {
++mp->part[i];
/* /\* special first ant *\/ */
/* if(i == 0 && part[i] > antpathlength[i]) { */
/* if(fir) */
/* first_ant_step = ant_step; */
/* antvelocity[i] = 0.0; */
/* /\* antposition[i][2] += 0.025; *\/ */
/* elevator += 0.025; */
/* /\* set light *\/ */
/* double l1 = 1.0 - (elevator < 1.0 ? elevator : 2.0 - elevator); */
/* GLfloat df[4] = {0.8*l1, 0.8*l1, 0.8*l1, 1.0}; */
/* glLightfv(GL_LIGHT0, GL_DIFFUSE, df); */
/* glLightfv(GL_LIGHT1, GL_DIFFUSE, df); */
/* /\* draw next board *\/ */
/* if(elevator > 1.0) { */
/* if(makenew == 1) { */
/* int re; */
/* /\* switch boards: rebuild old board, increment current *\/ */
/* currentboard = (currentboard+1)%BOARDCOUNT; */
/* build_board(currentboard); */
/* for(re = 1; re < ANTCOUNT; ++re) { */
/* anton[re] = 0; */
/* antmaterial[re] = materials[random()%MATERIALS]; */
/* } */
/* makenew = 0; */
/* } */
/* /\* draw the other board *\/ */
/* glEnable(GL_TEXTURE_2D); */
/* glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, MaterialGray6); */
/* glPushMatrix(); */
/* glTranslatef(-(-(BOARDSIZE-3.5)+(BOARDSIZE-1)/2.0), 0.0, */
/* -(2.4+BOARDSIZE+(BOARDSIZE-1)/2.0)); */
/* draw_board(mi, mp); */
/* glPopMatrix(); */
/* glDisable(GL_TEXTURE_2D); */
/* } */
/* /\* reset *\/ */
/* if(elevator > 2.0) { */
/* antposition[i][0] = -4.0;/\*-= -(-(BOARDSIZE-3.5)+(BOARDSIZE-1)/2.0);*\//\*= -4.0;*\/ */
/* antposition[i][1] = 5.5;/\*-(2.4+BOARDSIZE+(BOARDSIZE-1)/2.0);*\/ */
/* /\* antposition[i][2] = 0.15; *\/ */
/* antdirection[i] = PI/2.0; */
/* bposition[i][0] = 0; */
/* bposition[i][1] = 8; */
/* part[i] = 0; */
/* antvelocity[i] = 0.02; */
/* fir = 0; */
/* antmaterial[i] = MaterialRed; */
/* makenew = 1; */
/* elevator = 0.0; */
/* introduced = 200; */
/* } */
/* else { */
/* part[i]--; */
/* } */
/* } */
}
/* move toward goal, correct ant direction if required */
else {
/* difference */
double dx = mp->antpath[i][mp->part[i]][0] - mp->antposition[i][0];
double dz = - mp->antpath[i][mp->part[i]][1] + mp->antposition[i][1];
double theta, ideal;
if(dz > EPSILON)
theta = atan(dz/dx);
else
theta = dx > EPSILON ? 0.0 : PI;
ideal = theta - mp->antdirection[i];
if(ideal < -Pi/2.0)
ideal += Pi;
/* compute correction */
{
double dt = sign(ideal) * min(fabs(ideal), PI/90.0);
mp->antdirection[i] += dt;
if(mp->antdirection[i] > 2.0*PI)
mp->antdirection[i] = 0.0;
}
}
mp->antposition[i][0] += mp->antvelocity[i] * cos(mp->antdirection[i]);
mp->antposition[i][1] += mp->antvelocity[i] * sin(-mp->antdirection[i]);
}
}
void InstrumentGrid::setGamma(float gamma) {
MessageManagerLock l;
gamma_ = gamma;
useGamma_ = not near(gamma_, 1.0f);
}
/*Main program*/
void main()
{
char filename[100],ch,ans;
int selection,width,height;
const char * source_fileName;
const char * modified_fileName = "output.pgm";
const char * inte_fileName = "intermediate.pgm";
printf("\n************WELCOME***************");
printf("\nEnter image file to read(pgm format) : ");
scanf("%s",&filename);
source_fileName=filename;
printf("Reading image \"%s\" ...\n", source_fileName);
do
{
orig = readPGM(source_fileName, orig);
printf("\nDo you want to Enter Window size for the image?(Y/N)");
printf("\nEnter your ans here:");
getchar();
scanf("%c",&ans);
if(ans == 'y' || ans =='Y')
{
printf("\nEnter width < %d of window here:",orig->row);
scanf("%d",&width);
printf("\nEnter height < %d of window here:",orig->col);
scanf("%d",&height);
imgdata=&ImageData;
imgdata->row=width;
imgdata->col=height;
imgdata->max_gray=orig->max_gray;
imgdata->matrix = allocate_dynamic_matrix(imgdata->row,imgdata->col);
for(int m=0;m<width;m++)
for(int n=0;n<height;n++)
{
imgdata->matrix[m][n]=orig->matrix[m][n];
}
}
else
imgdata = orig;
printf("\n*********Image Scaling Options*************");
printf("\n\t1.Nearest Neighbour Method");
printf("\n\t2.Bilinear Interpolation");
printf("\nEnter your selection here:");
scanf("%d",&selection);
switch (selection)
{
case 1: near(orig);
break;
case 2: bipol(orig);
break;
default: printf("Invaild choice\n Exiting program...");
exit(1);
break;
}
writePGM(modified_fileName,newj);
writePGM(inte_fileName,imgdata);
printf("\nFile Writing complete");
printf("\nopening outputfile......");
/***** open a output file in openseeit ****/
char program[100] = "OpenSeeIt.exe ";
char *openme;
openme= strcat(program,modified_fileName);
system(openme);
printf("\nDo you want to continue?(Y/N)\nEnter your answer here:");
getchar();
ch=getchar();
}while(ch=='Y' || ch=='y');
}
void move(Uint8 id)
{
if (obj[id].AI >= 2)
{
if (near(255, id, 50))
{
if (obj[id].pos.x > oki->pos.x) obj[id].dir = DIR_LEFT;
else obj[id].dir = DIR_RIGHT;
if (obj[id].pos.x >= oki->pos.x-obj[id].speed &&
obj[id].pos.x <= oki->pos.x+obj[id].speed)
{
obj[id].dir = DIR_ZERO;
if (obj[id].img != events) obj[id].anim.x = 0;
}
if (obj[id].strength+2 < oki->strength && obj[id].type == OBJ_ENEMY)
obj[id].dir = (obj[id].dir==DIR_LEFT)*2;
} else
{
if (obj[id].dir == DIR_ZERO) obj[id].dir = DIR_LEFT;
}
}
if (obj[id].jump.power && near(255, id, 50))
{
if (obj[id].pos.y > oki->pos.y && obj[id].state != STATE_FALL &&
!obj[id].jump.frame) obj[id].jump.frame = 1;
if (obj[id].strength+2 < oki->strength && obj[id].type == OBJ_ENEMY)
obj[id].jump.frame = 0;
}
if (obj[id].run && id != 255)
{
if (near(255, id, 50))
{
if (obj[id].run == 1)
{
obj[id].speed=obj[id].speed*2-1;
obj[id].run = 2;
}
} else
{
if (obj[id].run == 2)
{
obj[id].speed=obj[id].speed/2+1;
obj[id].run = 1;
}
}
}
if (obj[id].speed >= 10)
{
obj[id].old_pos = obj[id].pos;
obj[id].old_anim = obj[id].anim;
}
if (obj[id].mass != 0)
{
if (!press(id))
{
if (obj[id].state != STATE_JUMP )
{
if (obj[id].img != events) obj[id].anim.x = 0;
obj[id].pos.y+=obj[id].fall;
obj[id].state = STATE_FALL;
if (obj[id].fall != 0 && obj[id].fall <= 9) obj[id].fall++;
if (obj[id].fall == 0) obj[id].fall = obj[id].mass;
go(id);
}
}
else
{
if (obj[id].fall > 8) obj[id].fall = 0;
if (obj[id].dir == DIR_ZERO && obj[id].img != events) obj[id].anim.x = 0;
obj[id].state = STATE_STAY;
}
}
if (obj[id].jump.frame > 0)
{
if (obj[id].jump.frame == 1 && id == 255) play_snd(jump);
obj[id].state = STATE_JUMP;
if (obj[id].img != events) obj[id].anim.x = 3*obj[id].pos.w;
obj[id].pos.y-=obj[id].jump.power;
obj[id].jump.frame++;
if (hit(id)) obj[id].jump.frame = obj[id].jump.time;
if (obj[id].jump.frame >= obj[id].jump.time)
{
obj[id].state = STATE_FALL;
if (obj[id].img != events) obj[id].anim.x = 0;
obj[id].jump.frame = 0;
obj[id].fall = 1;
}
go(id);
}
if (obj[id].mass == 0) obj[id].state = STATE_STAY;
if (obj[id].state == STATE_STAY && obj[id].dir != DIR_ZERO)
{
obj[id].state = STATE_WALK;
if (obj[id].img != events) obj[id].anim.x+=obj[id].anim.w;
if (obj[id].anim.x > 2*BLOCK_WIDTH && obj[id].img != events)
obj[id].anim.x = 0;
go(id);
}
if (obj[id].mass != 0) press(id);
if (obj[id].pos.x < 0)
{
obj[id].pos.x = 0;
if (obj[id].AI) obj[id].dir = DIR_RIGHT;
}
if (obj[id].pos.y < 0) obj[id].pos.y = 0;
if (obj[id].pos.x > game->w-obj[id].pos.w)
{
obj[id].pos.x = game->w-obj[id].pos.w;
if (obj[id].AI) obj[id].dir = DIR_LEFT;
}
if (obj[id].pos.y > game->h)
{
if (id != 255)
{
play_snd(click);
obj[id].vis = 0;
if (obj[id].type == OBJ_ENEMY) coins+=obj[id].strength*obj[id].AI+1;
} else
{
play_snd(slap);
die = 1;
}
}
}
/* bool Dstar::replan()
* --------------------------
* Updates the costs for all cells and computes the shortest path to
* goal. Returns true if a path is found, false otherwise. The path is
* computed by doing a greedy search over the cost+g values in each
* cells. In order to get around the problem of the robot taking a
* path that is near a 45 degree angle to goal we break ties based on
* the metric euclidean(state, goal) + euclidean(state,start).
*/
bool Dstar::replan() {
path.clear();
int res = computeShortestPath();
// printf("res: %d ols: %d ohs: %d tk: [%f %f] sk: [%f %f] sgr: (%f,%f)\n",res,openList.size(),openHash.size(),openList.top().k.first,openList.top().k.second, s_start.k.first, s_start.k.second,getRHS(s_start),getG(s_start));
if (res < 0) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
list<state> n;
list<state>::iterator i;
state cur = s_start;
state prev = s_start;
if (isinf(getG(s_start))) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
// constructs the path
while(cur != s_goal) {
path.path.push_back(cur);
path.cost += cost(prev,cur);
getSucc(cur, n);
if (n.empty()) {
//fprintf(stderr, "NO PATH TO GOAL\n");
path.cost = INFINITY;
return false;
}
// choose the next node in the path by selecting the one with smallest
// g() + cost. Break ties by choosing the neighbour that is closest
// to the line between start and goal (i.e. smallest sum of Euclidean
// distances to start and goal).
double cmin = INFINITY;
double tmin = INFINITY;
state smin = cur;
for (i=n.begin(); i!=n.end(); i++) {
if (occupied(*i)) continue;
double val = cost(cur,*i);
double val2 = trueDist(*i,s_goal) + trueDist(s_start,*i); // (Euclidean) cost to goal + cost to pred
double val3 = getG(*i);
val += val3;
// tiebreak if curent neighbour is equal to current best
// choose the neighbour that has the smallest tmin value
if (!isinf(val) && near(val,cmin)) {
if (val2 < tmin) {
tmin = val2;
cmin = val;
smin = *i;
}
}
// if next neighbour (*i) is scrictly lower cost than the
// current best, then set it to be the current best.
else if (val < cmin) {
tmin = val2;
cmin = val;
smin = *i;
}
} // end for loop
n.clear();
if( isinf(cmin) ) break;
prev = cur;
cur = smin;
} // end while loop
path.path.push_back(s_goal);
path.cost += cost(prev,s_goal);
return true;
}