Пример #1
0
inline void pointdynamics(point_t *curpt)           // Calculate point movement
{
    GLdouble r = vnorm(&curpt->pos);
    vector_t grav;
    vector_t rotf;
    vector_t coriolis;

    if(r > c_Re) {                                  // Points outside Earth
        grav = vscale(-c_Ge * curpt->m / pow(r,3), &curpt->pos);
        vaddto(&curpt->f, &grav);
    } else {                                        // Take care of points that
        vector_t scaled;                            // are inside Earth
        curpt->pos = vscale(c_Re / vnorm(&curpt->pos), &curpt->pos);
        scaled = vscale(-vdot(&curpt->pos, &curpt->v) / c_Re / c_Re, &curpt->pos);
        vaddto(&curpt->v, &scaled);
        curpt->v = vscale(0, &curpt->v);
    }

    rotf = vscale(c_Vang * c_Vang * curpt->m, &curpt->pos);
    rotf.z = 0;
    vset(&coriolis, 2 * curpt->v.y * curpt->m * c_Vang, -2 * curpt->v.x * curpt->m * c_Vang, 0);
    vaddto(&curpt->f, &rotf);
    vaddto(&curpt->f, &coriolis);
    vaddscaled(&curpt->v, TIMESTEP / curpt->m, &curpt->f);
    vaddscaled(&curpt->pos, TIMESTEP, &curpt->v);
    if(curtime <= 0) { curpt->v = vscale(.99, &curpt->v); }
}
Пример #2
0
void UpdateCamera() {
	vsub(&cameraPtr->dir, &cameraPtr->target, &cameraPtr->orig);
	vnorm(&cameraPtr->dir);

	const Vec up = {0.f, 1.f, 0.f};
	const float fov = (M_PI / 180.f) * 45.f;
	vxcross(&cameraPtr->x, &cameraPtr->dir, &up);
	vnorm(&cameraPtr->x);
	vsmul(&cameraPtr->x, width * fov / height, &cameraPtr->x);

	vxcross(&cameraPtr->y, &cameraPtr->x, &cameraPtr->dir);
	vnorm(&cameraPtr->y);
	vsmul(&cameraPtr->y, fov, &cameraPtr->y);
}
Пример #3
0
msym_error_t findSymmetryOperations(int esl, msym_equivalence_set_t es[], msym_thresholds_t *t, int *lsops, msym_symmetry_operation_t **sops){
    msym_error_t ret = MSYM_SUCCESS;
    msym_symmetry_operation_t *rsops = NULL;
    int lrsops = 0;
    for(int i = 0; i < esl;i++){
        int llsops = lrsops;
        if(MSYM_SUCCESS != (ret = findEquivalenceSetSymmetryOperations(&es[i], t, &lrsops, &rsops))) goto err;
        
        if(llsops > 0 && lrsops == 0) {
            free(rsops);
            rsops = NULL;
            break;
        }
    }
    
    for(int i = 0;i < lrsops;i++){
        vnorm(rsops[i].v);
    }
    
    *lsops = lrsops;
    *sops = rsops;
    return ret;
err:
    free(rsops);
    *sops = NULL;
    *lsops = 0;
    return ret;
}
Пример #4
0
void calcnormals(Gobject *o, int mallocnormals){

  int    aktpoly = -1,
         edges,i1,i2,i3,
         normanz = 0,
         aktnorm = 0;
  
  vec    norm;
  
  double erg;       
         
  while ((edges = o->polygons[++aktpoly])!=0){
    normanz++;
    aktpoly+=edges+2;
  }
/*  printf("Calcnormals.Normanzahl:%i\n",normanz);*/
  if (mallocnormals)
    o->na=malloc(sizeof(vec)*normanz);
  aktpoly=0;  
  for (aktnorm=0;aktnorm<normanz;aktnorm++){
    i1=o->polygons[aktpoly+3];
    i2=o->polygons[aktpoly+4];
    i3=o->polygons[aktpoly+5];
    veq(norm,vnorm(vp(vdiff(o->va[i2],o->va[i1]),vdiff(o->va[i3],o->va[i1]))));
    if (o->type==V3DPOLYHEDRON){
      erg=sp(vdiff(o->va[i1],o->posmc),norm);vreset();
      if (erg<0) veq(norm,sm(-1,norm));
      if (erg==0){printf("Fehler in calcnormals: Normale nicht definiert\n"); }
    }
/*    pv(norm);*/
    veq(o->na[aktnorm],norm);
    o->polygons[aktpoly+1]=aktnorm;
    aktpoly+=o->polygons[aktpoly]+3;
  }
}
Пример #5
0
void SlideNavmesh::update(const float dt)
{
	if (!m_update && !m_step)
		return;
	m_step = false;

	const float maxSpeed = 1.0f;
	NavmeshAgent* agent = &m_scene.agents[0];
	
	// Find next corner to steer to.
	// Smooth corner finding does a little bit of magic to calculate spline
	// like curve (or first tangent) based on current position and movement direction
	// next couple of corners.
	float corner[2],dir[2];
	int last = 1;
	vsub(dir, agent->pos, agent->oldpos); // This delta handles wall-hugging better than using current velocity.
	vnorm(dir);
	vcpy(corner, agent->pos);
	if (m_moveMode == AGENTMOVE_SMOOTH || m_moveMode == AGENTMOVE_DRUNK)
		last = agentFindNextCornerSmooth(agent, dir, m_scene.nav, corner);
	else
		last = agentFindNextCorner(agent, m_scene.nav, corner);
		
	if (last && vdist(agent->pos, corner) < 0.02f)
	{
		// Reached goal
		vcpy(agent->oldpos, agent->pos);
		vset(agent->dvel, 0,0);
		vcpy(agent->vel, agent->dvel);
		return;
	}

	vsub(agent->dvel, corner, agent->pos);

	// Apply style
	if (m_moveMode == AGENTMOVE_DRUNK)
	{
		agent->t += dt*4;
		float amp = cosf(agent->t)*0.25f;
		float nx = -agent->dvel[1];
		float ny = agent->dvel[0];
		agent->dvel[0] += nx * amp;
		agent->dvel[1] += ny * amp;
	}
	
	// Limit desired velocity to max speed.
	const float distToTarget = vdist(agent->pos,agent->target);
	const float clampedSpeed = maxSpeed * min(1.0f, distToTarget/agent->rad);
	vsetlen(agent->dvel, clampedSpeed);

	vcpy(agent->vel, agent->dvel);

	// Move agent
	vscale(agent->delta, agent->vel, dt);
	float npos[2];
	vadd(npos, agent->pos, agent->delta);
	agentMoveAndAdjustCorridor(&m_scene.agents[0], npos, m_scene.nav);

}
void UpdateRenderingCPU(void) {
	double startTime = WallClockTime();

	const float invWidth = 1.f / width;
	const float invHeight = 1.f / height;

	int x, y;
	for (y = 0; y < height; y++) { /* Loop over image rows */
		for (x = 0; x < width; x++) { /* Loop cols */
			const int i = (height - y - 1) * width + x;
			const int i2 = 2 * i;

			const float r1 = GetRandom(&seeds[i2], &seeds[i2 + 1]) - .5f;
			const float r2 = GetRandom(&seeds[i2], &seeds[i2 + 1]) - .5f;
			const float kcx = (x + r1) * invWidth - .5f;
			const float kcy = (y + r2) * invHeight - .5f;

			Vec rdir;
			vinit(rdir,
				camera.x.x * kcx + camera.y.x * kcy + camera.dir.x,
				camera.x.y * kcx + camera.y.y * kcy + camera.dir.y,
				camera.x.z * kcx + camera.y.z * kcy + camera.dir.z);

			Vec rorig;
			vsmul(rorig, 0.1f, rdir);
			vadd(rorig, rorig, camera.orig)

			vnorm(rdir);
			const Ray ray = {rorig, rdir};
			Vec r;
			RadiancePathTracing(spheres, sphereCount, &ray,
					&seeds[i2], &seeds[i2 + 1], &r);

			if (currentSample == 0)
				colors[i] = r;
			else {
				const float k1 = currentSample;
				const float k2 = 1.f / (k1 + 1.f);
				colors[i].x = (colors[i].x * k1 + r.x) * k2;
				colors[i].y = (colors[i].y * k1 + r.y) * k2;
				colors[i].z = (colors[i].z * k1 + r.z) * k2;
			}

			pixels[y * width + x] = toInt(colors[i].x) |
					(toInt(colors[i].y) << 8) |
					(toInt(colors[i].z) << 16);
		}
	}

	const float elapsedTime = WallClockTime() - startTime;
	const float sampleSec = height * width / elapsedTime;
	sprintf(captionBuffer, "Rendering time %.3f sec (pass %d)  Sample/sec  %.1fK\n",
		elapsedTime, currentSample, sampleSec / 1000.f);

	currentSample++;
}
Пример #7
0
/** \brief thresholding as proposed in the Matlab Wavelet-Toolbox.

	 \ingroup grpwavelet
    \code
       case 'heursure' 
         hthr = sqrt(2*log(n)); 
         eta = (norm(x).^2-n)/n; 
         crit = (log(n)/log(2))^(1.5)/sqrt(n); 
         if eta < crit 
             thr = hthr; 
         else 
             thr = min(thselect(x,'rigrsure'),hthr); 
         end 
	 \endcode
*/
double heuristic_sure(const double *data, int n){
  dprintf("Db: heuristic_sure\n");
  double hthr, etat, crit;
  hthr = sqrt(2*log(n));
  etat = (pow(vnorm(data, n, 2), 2)-n)/(double)n;
  crit = pow((log(n)/log(2)), 1.5)/sqrt(n);
  if(etat < crit)
    return hthr;
  else 
    return fmin(sureshrink(data, n), hthr);
}
Пример #8
0
/* bond energy 1/2 k (r - r0)^2 */
static double potbond(double *a, double *b,
    double r0, double kr, double *fa, double *fb)
{
  double dx[D], r, dr, amp;

  r = vnorm( vdiff(dx, a, b) );
  dr = r - r0;
  amp = kr * dr / r;
  vsinc(fa, dx, -amp);
  vsinc(fb, dx,  amp);
  return 0.5 * kr * dr * dr;
}
Пример #9
0
int le_save_task(le_task *t, const char *file)
{
	int i, j;
	FILE *fp = fopen(file, "w");
	if (fp == NULL) {
		perror("Failed to open file");
		return 1;
	}
	fprintf(fp, "# vtk DataFile Version 3.0\n");
	fprintf(fp, "Created by le_save_task\n");
	fprintf(fp, "BINARY\n");
	fprintf(fp, "DATASET STRUCTURED_POINTS\n");
	fprintf(fp, "DIMENSIONS %d %d 1\n", t->n.x, t->n.y);
	fprintf(fp, "SPACING %f %f 0.0\n", t->h.x, t->h.y);
	fprintf(fp, "ORIGIN 0.0 0.0 0.0\n");
	fprintf(fp, "POINT_DATA %d\n", t->n.x * t->n.y);
	
	/* velocity */
	fprintf(fp, "SCALARS v float 1\n");
	fprintf(fp, "LOOKUP_TABLE v_table\n");
	for (j = 0; j < t->n.y; j++) {
		for (i = 0; i < t->n.x; i++) {
			float v;
			if (t->stype == ST_AOS) {
				v = vnorm(gind(i, j).v);
			} else if (t->stype == ST_SOA) {
				le_vec2 vt = { soa_vx(i, j), soa_vy(i, j) };
				v = vnorm(vt);
			} else {
				assert(0);
			}
			write_float(fp, v);
		}
	}
	/*
	 * You can use the same code for saving other variables.
	 */
	fclose(fp);
	return 0;
}
Пример #10
0
void simul()
{
    for(int i = 0; i < numpoints; i++) { vset(&points[i].f, 0, 0, 0); }

    for(int i = 0; i < numpoints - 1; i++) {
        vector_t vdiff;
        GLdouble axialv, frict, elast;

        vector_t diff = vsub(&points[i+1].pos, &points[i].pos);
        GLdouble l = vnorm(&diff);
        vector_t f;

        elast = points[i].k * (1 - points[i].initlen / l);
        if(elast < 0) { elast = 0; }
        points[i].tension = elast * l;

        vdiff = vsub(&points[i+1].v, &points[i].v);
        axialv = vdot(&vdiff, &diff) / l;
        frict = axialv / l * points[i].frict;

        f = vscale(elast + frict, &diff);
        vaddto(&points[i].f, &f);
        vsubfrom(&points[i+1].f, &f);
    }

    for(int i = 0; i < numpoints; i++) {
        double loading = points[i].tension / points[i].crosssect / c_Slim;
        if(loading > 1 && curtime > 0) {
            points[i].k = 0;
            points[i].frict = 0;
        }
    }

    for(int i = 0; i < numpoints; i++) { pointdynamics(&points[i]); }

    {
        points[0].pos.x = c_Re * cos(0 / 180.0 * M_PI);
        points[0].pos.y = 0;
        points[0].pos.z = c_Re * sin(0 / 180.0 * M_PI);
        vset(&points[0].v, 0, 0, 0);
        if(curtime > 1) {
            points[BREAKPOINT].k = 0;
            points[BREAKPOINT].frict = 0;
        }
    }

    curtime += TIMESTEP;
}
Пример #11
0
double DistanceTransform::distance2Triangle(const Point3f& pnt, int nt, Point3f& nearPnt)
{
	double  dist, temp[3];
	Point3f v0, v1, v2;
	p_Surf->getTriVerts(nt, v0, v1, v2);
	Vector3f norm;
	p_Surf->getTriNormal(nt, norm);
	vector3 vnorm(norm[0], norm[1], norm[2]), vdiff;
	
	Point3f edgePnt;

	//1) First compute the shortest distance from the pnt to the plane of the Triangle.
	vdiff[0] = pnt[0] - v0[0];
	vdiff[1] = pnt[1] - v0[1];
	vdiff[2] = pnt[2] - v0[2];
	dist = DotProduct(vdiff, vnorm);
	nearPnt[0] = pnt[0] - dist * norm[0];
	nearPnt[1] = pnt[1] - dist * norm[1];
	nearPnt[2] = pnt[2] - dist * norm[2];

	//2) Then check if the projected point is within the triangle or not. 
	//	  if yes, then the above is the correct shortest distance.
	if(pointInTriangle(nearPnt, v0, v1, v2, norm)) {
		return fabs(dist);
	}

    //3) now, the closest point must on the edge.
	// find the nearest point on each edge.
	dist = distance2Edge(pnt, v0, v1, nearPnt);
	double edgeDist = distance2Edge(pnt, v1, v2, edgePnt);
	if(edgeDist < dist) {
		dist = edgeDist;
		nearPnt = edgePnt;
	}
	edgeDist = distance2Edge(pnt, v2, v0, edgePnt);
	if(edgeDist < dist) {
		dist = edgeDist;
		nearPnt = edgePnt;
	}
	
	assert(dist >= 0);
	return dist;
}
Пример #12
0
void draw_thick_curve_segment(float t, vector *pos, vector *dir, void *args) {
  draw_thick_curve_segment_args *dtcsargs=(draw_thick_curve_segment_args*) args;
  // Compute a vector from the point pos to the edge of the current segment:
  vector toedge;
  vcopy_as(&toedge, dir);
  vrotz(&toedge, M_PI_2);
  vnorm(&toedge);
  float width = (*(dtcsargs->width_func))(t, dtcsargs->width_param);
  vscale(&toedge, width/2.0);

  curve segment; // actually just a straight line...

  segment.from.x = pos->x - toedge.x; // from one edge
  segment.from.y = pos->y - toedge.y;
  segment.from.z = 0;
  segment.come_from.x = pos->x - toedge.x;
  segment.come_from.y = pos->y - toedge.y;
  segment.come_from.z = 0;

  segment.go_towards.x = pos->x + toedge.x; // to the other
  segment.go_towards.y = pos->y + toedge.y;
  segment.go_towards.z = 0;
  segment.to.x = pos->x + toedge.x;
  segment.to.y = pos->y + toedge.y;
  segment.to.z = 0;

  // Draw our segment:
  draw_curve(
    dtcsargs->tx,
    &segment,
    dtcsargs->color
  );

  // Draw the shade pixel:
  tx_set_px(
    dtcsargs->tx,
    dtcsargs->shade_color,
    (size_t) segment.from.x,
    (size_t) segment.from.y
  );
}
Пример #13
0
void contour3d(Contour* c){
  int i,j,*k;
  vecp v;
  matp m;
  if (c->initialized==0){
    c->tva=malloc((c->vaakt+1)*sizeof(vec));
    c->na=malloc((c->polakt+1)*sizeof(vec));
    c->sp=malloc((c->polakt+1)*sizeof(vec));
    c->points=malloc((c->polakt+1)*sizeof(DPoint));    
    c->Garray=malloc((c->polakt+1)*sizeof(Gobject));
    for (i=0;i<=c->polakt;i++) {
      c->Garray[i].na=c->na;
      c->Garray[i].tva=c->tva;
      c->Garray[i].poswc=(vecp) &c->sp[i]; /* compiler distinguishes between
                                              vec* and vecp */
      c->Garray[i].points=c->points;
      c->Garray[i].polygons=&c->pol[i*6];
      c->Garray[i].drawstruct=drawtrianglelight;
      j=c->pol[i*6];
      veq(c->na[i],vnorm(vp(vdiff(c->va[j+4],c->va[j+3]),vdiff(c->va[j+5],c->va[j+3]))));
    }
    c->initialized=1;
  }
  /* Transformation, die jedes Mal durchlaufen wird. */
  meq(c->mc2wcmn,getmc2wcmn());
  meq(c->mc2wcm,getmc2wcm());
  veq(c->mc2wcv,getmc2wcv());
  m=c->mc2wcm;
  v=c->mc2wcv;
  for (i=0;i<=c->vaakt;i++) {
    veq(c->tva[i],vsum(matvecmul(m,c->va[i]),v));
    wc2dc(&c->points[i].x,&c->points[i].y,c->tva[i]);
  }
  for (i=0;i<=c->polakt;i++){ /* Schwerpunkt in Weltkoordinaten */
    k=&c->pol[i*6+3];
    veq(c->sp[i],sm(0.33333,
		    vsum(vsum(c->tva[*k],c->tva[*(k+1)]),c->tva[*(k+2)])));
    insertGobject(&c->Garray[i]);
  }
}
Пример #14
0
void geRK4ApplyGravity(ge_RK4State* s1, ge_RK4State* s2){
	const double G = 6.67234E-11;

	double d = vdist(s1->position, s2->position);
	if(d <= 1.0 || d < s1->radius_min || d < s2->radius_min){
		return;
	}
	double P = G * ((s1->mass * s2->mass) / (d * d));
	ge_Vector3d force, f1, f2;

	force.x = s2->position.x - s1->position.x;
	force.y = s2->position.y - s1->position.y;
	force.z = s2->position.z - s1->position.z;
	force = vnorm(force);
	force = vmulk(force, P);
	f1.x = force.x;
	f1.y = force.y;
	f1.z = force.z;
	f2.x = -force.x;
	f2.y = -force.y;
	f2.z = -force.z;
	geRK4ApplyForce(s1, f1);
	geRK4ApplyForce(s2, f2);
}
Пример #15
0
void le_set_ball(le_task *t, const le_vec2 c, const real r, const real s)
{
	int i, j;
	for (i = 0; i < t->n.x; i++) {
		for (j = 0; j < t->n.y; j++) {
			le_vec2 x = {t->h.x * i, t->h.y * j};
			le_vec2 d = {x.x - c.x, x.y - c.y};
			if (vnorm(d) < r) {
				/*
				 * Set pressure disturbance,
				 */
				if (t->stype == ST_AOS) {
					gind(i, j).s.xx = s;
					gind(i, j).s.yy = s;
				} else if (t->stype == ST_SOA) {
					soa_sxx(i, j) = s;
					soa_syy(i, j) = s;
				} else {
					assert(0);
				}
			}
		}
	}
}
Пример #16
0
int
pt_in_gt (struct pt *p, struct groundtri *gt)
{
	double theta;
	struct vect v1, v2;

	theta = 0;

	psub (&v1, &gt->p1, p);
	vnorm (&v1, &v1);

	psub (&v2, &gt->p2, p);
	vnorm (&v2, &v2);

	theta += acos (vdot (&v1, &v2));


	psub (&v1, &gt->p2, p);
	vnorm (&v1, &v1);

	psub (&v2, &gt->p3, p);
	vnorm (&v2, &v2);

	theta += acos (vdot (&v1, &v2));

		
	psub (&v1, &gt->p3, p);
	vnorm (&v1, &v1);

	psub (&v2, &gt->p1, p);
	vnorm (&v2, &v2);

	theta += acos (vdot (&v1, &v2));

	if (theta >= DTOR ((360 - 1e-7))) {
		return (1);
	}

	return (0);
}
Пример #17
0
void
grounded_moving (void)
{
	double now, dt;
	struct pt newpos;
	struct groundtri *gt;
	struct vect ctrl_vel;

	now = get_secs ();
	dt = now - player.lasttime;

	ctrl_vel.x = 0;
	ctrl_vel.y = 0;
	ctrl_vel.z = 0;

	if (fabs (player.p.z - ground_height (&player.p)) <= 1e6) {
		switch (player.moving) {
		case BACK:
			ctrl_vel.x = -.3 * player.speed * cos (player.theta);
			ctrl_vel.y = -.3 * player.speed * sin (player.theta);
			break;
		case FORW:
			ctrl_vel.x = player.speed * cos (player.theta);
			ctrl_vel.y = player.speed * sin (player.theta);
			break;
		case SIDE_Q:
			ctrl_vel.x = player.speed
				* cos (player.theta + DTOR (90));
			ctrl_vel.y = player.speed
				* sin (player.theta + DTOR (90));
			break;
		case SIDE_E:
			ctrl_vel.x = player.speed
				* cos (player.theta - DTOR (90));
			ctrl_vel.y = player.speed
				* sin (player.theta - DTOR (90));
			break;
		case FORW_Q:
			ctrl_vel.x = player.speed
				* cos (player.theta + DTOR (45));
			ctrl_vel.y = player.speed
				* sin (player.theta + DTOR (45));
			break;
		case FORW_E:
			ctrl_vel.x = player.speed
				* cos (player.theta - DTOR (45));
			ctrl_vel.y = player.speed
				* sin (player.theta - DTOR (45));
			break;
		case BACK_Q:
			ctrl_vel.x = -.3 * player.speed
				* cos (player.theta - DTOR (45));
			ctrl_vel.y = -.3 * player.speed
				* sin (player.theta - DTOR (45));
			break;
		case BACK_E:
			ctrl_vel.x = -.3 * player.speed
				* cos (player.theta + DTOR (45));
			ctrl_vel.y = -.3 * player.speed
				* sin (player.theta + DTOR (45));
			break;
		}
	}

	newpos.x = player.p.x + ctrl_vel.x * dt;
	newpos.y = player.p.y + ctrl_vel.y * dt;
	newpos.z = player.p.z + ctrl_vel.z * dt;

	psub (&player.vel, &newpos, &player.p);
	vscal (&player.vel, &player.vel, 1 / dt);

	if ((gt = detect_plane (&newpos)) == NULL) {
		printf ("Can't find ground, moving player to start position "
			"to avoid seg fault\n");
		player.p.x = 0;
		player.p.y = 0;
		player.p.z = ground_height (&player.p);
		player.movtype = GROUNDED;
		player.loc = detect_plane (&player.p);
		return;
	}

	if (gt->theta < DTOR (50)) {
		newpos.z = ground_height (&newpos);
		psub (&player.vel, &newpos, &player.p);
		vscal (&player.vel, &player.vel, 1 / dt);
		player.p = newpos;
	} else if (newpos.z > gt->pl.middle.z) {
		//imperfect detection of cliffs but tolerable for now
		player.p = newpos;
		player.movtype = FALLING;
		printf ("switching to falling\n");
	} else {
		if (gt == player.loc) {
			printf ("line %d: HORRIBLE MESSINESS\n", __LINE__);
			paused = YES;
			printf ("paused game\n");
			return;
		}

		struct vect v1, v2;

		vnorm (&v2, &ctrl_vel);
		vcross (&v1, &player.loc->normv, &gt->normv);
		vnorm (&v1, &v1);
		if (vdot (&v1, &v2) < 0)
			vscal (&v1, &v1, -1);
		vscal (&ctrl_vel, &v1, vdot (&ctrl_vel, &v1));
			
		newpos.x = player.p.x + ctrl_vel.x * dt;
		newpos.y = player.p.y + ctrl_vel.y * dt;
		newpos.z = player.p.z + ctrl_vel.z * dt;

		newpos.z = ground_height (&newpos);
		psub (&player.vel, &newpos, &player.p);
		vscal (&player.vel, &player.vel, 1 / dt);
				
		player.p = newpos;
	}
}
Пример #18
0
/******************************************************************
 * Solve Inverse Kinematics
 ******************************************************************/
double sKinematics::solveIKPalmItr(double x_dest[], int axis, double dir_dest[], double q_rest[], double q_out[])
{	
	int i,j;
	double *q_old, *q_curr, *q_delta, *q_null;
	double x[3], dir[3];
	double ox[6];
	double error_old, error;

	error_old = 1000.0; // set maximum error;

	q_old  = svector(dof);
	q_curr = svector(dof);
	q_delta= svector(dof);
	q_null = svector(dof);

	while(1){
		getJacobianPalm(axis, OJ );
		getEndPos(x);
		getEndDirAxis(axis, dir);
		getJoints(q_curr);

		for( i=0; i<3; i++ ){
			ox[i  ] = x_dest[i]-x[i];
			ox[i+3] = dir_dest[i] - dir[i];
		}
		error = vnorm(6, ox );
		//printf("%f \n", error );

		// stop when the error reach to a local minimum
		if( error < IK_TOLERANCE ){
			matcp_c1(dof, q_curr, q_out );
			return error;
		}
		else if( abs(error_old - error)  < IK_TOLERANCE ){
			matcp_c1(dof, q_curr, q_out );
			return error;
		}
		else if( error > error_old ){
			matcp_c1(dof, q_old, q_out );
			return error_old;
		}

		matcp_c1(dof, q_curr, q_old );

		// solve inverse kinematics
		matsvdinvDLS(OJ, 6, dof, 0.01, OJi ); 
		// set weight
		for( i=0; i<dof; i++ ){
			for( j=0; j<6; j++ ){
				OJi[i][j] *= sDH[active_index[i]].weight;
			}
		}

		matnull(6, dof, OJ, OJi, N );               
		matsub_c1(dof, q_rest, q_curr, q_null );
		for(i=0; i<dof; i++ ) q_null[i] *= lamda;

		matmul_vec(OJi, dof, 6, ox, q_delta );
		matadd_c1(dof, q_delta, q_curr );

		matmul_vec( N, dof, dof, q_null, q_delta ); 
		matadd_c1(dof, q_delta, q_curr );           


		checkVelocityLimit(q_old, q_curr, deltaT);
		setJoints(q_curr);
		
		getEndPos(x);
		getEndDirAxis(axis, dir);
		for( i=0; i<3; i++ ){
			ox[i  ] = x_dest[i]-x[i];
			ox[i+3] = dir_dest[i] - dir[i];
		}
		error = vnorm(6, ox );
		error_old = error;		
		
    }
}
Пример #19
0
void
old_grounded_moving (void)
{
	double now, dt;
	struct pt newpos;
	struct groundtri *gt;
	struct vect v1, v2, ctrl_vel;

	now = get_secs ();
	dt = now - player.lasttime;

	ctrl_vel.x = 0;
	ctrl_vel.y = 0;
	ctrl_vel.z = 0;

	if (player.p.z <= ground_height (&player.p)) {
		switch (player.moving) {
		case BACK:
			ctrl_vel.x = -.3 * player.speed * cos (player.theta);
			ctrl_vel.y = -.3 * player.speed * sin (player.theta);
			break;
		case FORW:
			ctrl_vel.x = player.speed * cos (player.theta);
			ctrl_vel.y = player.speed * sin (player.theta);
			break;
		case SIDE_Q:
			ctrl_vel.x = player.speed
				* cos (player.theta + DTOR (90));
			ctrl_vel.y = player.speed
				* sin (player.theta + DTOR (90));
			break;
		case SIDE_E:
			ctrl_vel.x = player.speed
				* cos (player.theta - DTOR (90));
			ctrl_vel.y = player.speed
				* sin (player.theta - DTOR (90));
			break;
		case FORW_Q:
			ctrl_vel.x = player.speed
				* cos (player.theta + DTOR (45));
			ctrl_vel.y = player.speed
				* sin (player.theta + DTOR (45));
			break;
		case FORW_E:
			ctrl_vel.x = player.speed
				* cos (player.theta - DTOR (45));
			ctrl_vel.y = player.speed
				* sin (player.theta - DTOR (45));
			break;
		case BACK_Q:
			ctrl_vel.x = -.3 * player.speed
				* cos (player.theta - DTOR (45));
			ctrl_vel.y = -.3 * player.speed
				* sin (player.theta - DTOR (45));
			break;
		case BACK_E:
			ctrl_vel.x = -.3 * player.speed
				* cos (player.theta + DTOR (45));
			ctrl_vel.y = -.3 * player.speed
				* sin (player.theta + DTOR (45));
			break;
		}
	}

	newpos.x = player.p.x + ctrl_vel.x * dt;
	newpos.y = player.p.y + ctrl_vel.y * dt;
	newpos.z = player.p.z + ctrl_vel.z * dt;

	psub (&player.vel, &newpos, &player.p);
	vscal (&player.vel, &player.vel, 1 / dt);

	if ((gt = detect_plane (&newpos)) == NULL) {
		printf ("Can't find ground, moving player to start position "
			"to avoid seg fault\n");
		player.p.x = 0;
		player.p.y = 0;
		player.p.z = ground_height (&player.p);
		player.movtype = GROUNDED;
		player.loc = detect_plane (&player.p);
		return;
	}

	if (gt->theta < DTOR (50)) {
		newpos.z = ground_height (&newpos);
		psub (&player.vel, &newpos, &player.p);
		vscal (&player.vel, &player.vel, 1 / dt);
		player.p = newpos;
	} else if (newpos.z > gt->pl.middle.z) {
		player.p = newpos;
		player.movtype = FALLING;
		printf ("switching to falling\n");
	} else if (newpos.z <= gt->pl.middle.z) {
		if (gt == player.loc) {
			printf ("line %d: some sort of bug caused"
				" player to be in grounded mode while"
				" on steep plane\n", __LINE__);
			pt_on_z_plane (&newpos, &newpos, &player.loc->pl);
			player.p = newpos;
			printf ("shifting player to nearest point on"
				" plane with same z\n");
			vset (&player.vel, 0, 0, 0);
			printf ("killing player velocity\n");
			player.movtype = FALLING;
			printf ("switching to falling\n");
			paused = YES;
			printf ("paused game\n");
			return;
		}

		vnorm (&v2, &ctrl_vel);
		vcross (&v1, &player.loc->normv, &gt->normv);
		vnorm (&v1, &v1);
		if (vdot (&v1, &v2) < 0)
			vscal (&v1, &v1, -1);
		vscal (&ctrl_vel, &v1, vdot (&ctrl_vel, &v1));
			
		newpos.x = player.p.x + ctrl_vel.x * dt;
		newpos.y = player.p.y + ctrl_vel.y * dt;
		newpos.z = player.p.z + ctrl_vel.z * dt;
			
		if ((gt = detect_plane (&newpos)) == NULL) {
			printf ("can't detect plane, exiting\n");
			exit (1);
		}
			
		if (gt->theta < DTOR (50)) {
			newpos.z = ground_height (&newpos);
			psub (&player.vel, &newpos, &player.p);
			vscal (&player.vel, &player.vel, 1 / dt);
				
			printf ("bar\n");

			player.p = newpos;
		} else {
//			struct groundtri *first_col, *last_col, *gt2;
//			int i;

			//PLAYER STARTS MOVING INTO GROUND BEFORE THIS EVER
			//HAPPENS

			printf ("pausing\n");
			paused = YES;

			/* i = 0; */
			/* first_col = NULL; */
			/* last_col = NULL; */

			/* for (gt2 = first_groundtri; gt2; gt2 = gt2->next) { */
			/* 	if (detect_collision (&player.p, */
			/* 			      &newpos, gt2)) { */
			/* 		if (first_col == NULL) { */
			/* 			first_col = gt2; */
			/* 		} else { */
			/* 			last_col->next = gt2; */
			/* 		} */
			/* 		last_col = gt2; */

			/* 		i++; */
			/* 	} */
			/* } */

			/* if (first_col == NULL) { */
			/* 	printf ("%s: %s: %d: something weird\n", */
			/* 		__FILE__, __FUNCTION__, __LINE__); */
			/* } */
			
			/* printf ("number of collisions: %d\n", i); */



			//TRY PROJECTING PLAYER TO LINE OF INTERSECTION
			//BETWEEN OLD & NEW PLANES. IF PLAYER IS STILL
			//INSIDE ANY PLANES, GRAB THREE PLANES AND GO
			//TO INTERSECTION POINT OF THOSE. MIGHT WANT
			//TO CHECK FOR OVER THREE PLANES JUST IN CASE

/* 			printf ("player.z: %g\n", player.p.z); */
/* 			newpos = player.p; */
/* 			rnd_escape (&newpos, gt); */
/* 			printf ("ran rnd_escape\n"); */
/* 			gt = detect_plane (&newpos); */


/* 			double b2; */
/* 			b2 = newpos.y - (gt->pl.b / gt->pl.a) * newpos.x; */
/* 			newpos.x = -(b2 * gt->pl.a * gt->pl.b */
/* 				     + gt->pl.c * newpos.z * gt->pl.a */
/* 				     + gt->pl.d * gt->pl.a) */
/* 				/ (square (gt->pl.a) + square (gt->pl.b)); */
/* 			newpos.y = (gt->pl.b / gt->pl.a) * newpos.x + b2; */
/* 			player.p = newpos; */
/* 			printf ("shifting player to nearest point on" */
/* 				" plane with same z\n"); */


/* 			gt = detect_plane (&player.p); */
/* 			printf ("re detected plane\n"); */
/* 			if (gt->theta >= DTOR (50)) { */
/* 				printf ("rnd_escape put player on slope," */
/* 					" changing player to falling\n"); */
/* 				player.movtype = FALLING; */
/* 				printf ("killing velocity\n"); */
/* 				vset (&player.vel, 0, 0, 0); */
/* 			} */
/* 			printf ("player.z: %g\n", player.p.z); */
		}
	} else {
		printf ("this should never be printed\n");
	}
}
Пример #20
0
/*************************************************************************
Testing Nearest Neighbor Search on uniformly distributed hypercube

NormType: 0, 1, 2
D: space dimension
N: points count
*************************************************************************/
static void testkdtuniform(const ap::real_2d_array& xy,
     const int& n,
     const int& nx,
     const int& ny,
     const int& normtype,
     bool& kdterrors)
{
    double errtol;
    ap::integer_1d_array tags;
    ap::real_1d_array ptx;
    ap::real_1d_array tmpx;
    ap::boolean_1d_array tmpb;
    kdtree treex;
    kdtree treexy;
    kdtree treext;
    ap::real_2d_array qx;
    ap::real_2d_array qxy;
    ap::integer_1d_array qtags;
    ap::real_1d_array qr;
    int kx;
    int kxy;
    int kt;
    int kr;
    double eps;
    int i;
    int j;
    int k;
    int task;
    bool isequal;
    double r;
    int q;
    int qcount;

    qcount = 10;
    
    //
    // Tol - roundoff error tolerance (for '>=' comparisons)
    //
    errtol = 100000*ap::machineepsilon;
    
    //
    // fill tags
    //
    tags.setlength(n);
    for(i = 0; i <= n-1; i++)
    {
        tags(i) = i;
    }
    
    //
    // build trees
    //
    kdtreebuild(xy, n, nx, 0, normtype, treex);
    kdtreebuild(xy, n, nx, ny, normtype, treexy);
    kdtreebuildtagged(xy, tags, n, nx, 0, normtype, treext);
    
    //
    // allocate arrays
    //
    tmpx.setlength(nx);
    tmpb.setlength(n);
    qx.setlength(n, nx);
    qxy.setlength(n, nx+ny);
    qtags.setlength(n);
    qr.setlength(n);
    ptx.setlength(nx);
    
    //
    // test general K-NN queries (with self-matches):
    // * compare results from different trees (must be equal) and
    //   check that correct (value,tag) pairs are returned
    // * test results from XT tree - let R be radius of query result.
    //   then all points not in result must be not closer than R.
    //
    for(q = 1; q <= qcount; q++)
    {
        
        //
        // Select K: 1..N
        //
        if( ap::fp_greater(ap::randomreal(),0.5) )
        {
            k = 1+ap::randominteger(n);
        }
        else
        {
            k = 1;
        }
        
        //
        // Select point (either one of the points, or random)
        //
        if( ap::fp_greater(ap::randomreal(),0.5) )
        {
            i = ap::randominteger(n);
            ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
        }
        else
        {
            for(i = 0; i <= nx-1; i++)
            {
                ptx(i) = 2*ap::randomreal()-1;
            }
        }
        
        //
        // Test:
        // * consistency of results from different queries
        // * points in query are IN the R-sphere (or at the boundary),
        //   and points not in query are outside of the R-sphere (or at the boundary)
        // * distances are correct and are ordered
        //
        kx = kdtreequeryknn(treex, ptx, k, true);
        kxy = kdtreequeryknn(treexy, ptx, k, true);
        kt = kdtreequeryknn(treext, ptx, k, true);
        if( kx!=k||kxy!=k||kt!=k )
        {
            kdterrors = true;
            return;
        }
        kx = 0;
        kxy = 0;
        kt = 0;
        kdtreequeryresultsx(treex, qx, kx);
        kdtreequeryresultsxy(treexy, qxy, kxy);
        kdtreequeryresultstags(treext, qtags, kt);
        kdtreequeryresultsdistances(treext, qr, kr);
        if( kx!=k||kxy!=k||kt!=k||kr!=k )
        {
            kdterrors = true;
            return;
        }
        kdterrors = kdterrors||kdtresultsdifferent(xy, n, qx, qxy, qtags, k, nx, ny);
        for(i = 0; i <= n-1; i++)
        {
            tmpb(i) = true;
        }
        r = 0;
        for(i = 0; i <= k-1; i++)
        {
            tmpb(qtags(i)) = false;
            ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
            ap::vsub(&tmpx(0), 1, &qx(i, 0), 1, ap::vlen(0,nx-1));
            r = ap::maxreal(r, vnorm(tmpx, nx, normtype));
        }
        for(i = 0; i <= n-1; i++)
        {
            if( tmpb(i) )
            {
                ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
                ap::vsub(&tmpx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
                kdterrors = kdterrors||ap::fp_less(vnorm(tmpx, nx, normtype),r*(1-errtol));
            }
        }
        for(i = 0; i <= k-2; i++)
        {
            kdterrors = kdterrors||ap::fp_greater(qr(i),qr(i+1));
        }
        for(i = 0; i <= k-1; i++)
        {
            ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
            ap::vsub(&tmpx(0), 1, &xy(qtags(i), 0), 1, ap::vlen(0,nx-1));
            kdterrors = kdterrors||ap::fp_greater(fabs(vnorm(tmpx, nx, normtype)-qr(i)),errtol);
        }
    }
    
    //
    // test general approximate K-NN queries (with self-matches):
    // * compare results from different trees (must be equal) and
    //   check that correct (value,tag) pairs are returned
    // * test results from XT tree - let R be radius of query result.
    //   then all points not in result must be not closer than R/(1+Eps).
    //
    for(q = 1; q <= qcount; q++)
    {
        
        //
        // Select K: 1..N
        //
        if( ap::fp_greater(ap::randomreal(),0.5) )
        {
            k = 1+ap::randominteger(n);
        }
        else
        {
            k = 1;
        }
        
        //
        // Select Eps
        //
        eps = 0.5+ap::randomreal();
        
        //
        // Select point (either one of the points, or random)
        //
        if( ap::fp_greater(ap::randomreal(),0.5) )
        {
            i = ap::randominteger(n);
            ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
        }
        else
        {
            for(i = 0; i <= nx-1; i++)
            {
                ptx(i) = 2*ap::randomreal()-1;
            }
        }
        
        //
        // Test:
        // * consistency of results from different queries
        // * points in query are IN the R-sphere (or at the boundary),
        //   and points not in query are outside of the R-sphere (or at the boundary)
        // * distances are correct and are ordered
        //
        kx = kdtreequeryaknn(treex, ptx, k, true, eps);
        kxy = kdtreequeryaknn(treexy, ptx, k, true, eps);
        kt = kdtreequeryaknn(treext, ptx, k, true, eps);
        if( kx!=k||kxy!=k||kt!=k )
        {
            kdterrors = true;
            return;
        }
        kx = 0;
        kxy = 0;
        kt = 0;
        kdtreequeryresultsx(treex, qx, kx);
        kdtreequeryresultsxy(treexy, qxy, kxy);
        kdtreequeryresultstags(treext, qtags, kt);
        kdtreequeryresultsdistances(treext, qr, kr);
        if( kx!=k||kxy!=k||kt!=k||kr!=k )
        {
            kdterrors = true;
            return;
        }
        kdterrors = kdterrors||kdtresultsdifferent(xy, n, qx, qxy, qtags, k, nx, ny);
        for(i = 0; i <= n-1; i++)
        {
            tmpb(i) = true;
        }
        r = 0;
        for(i = 0; i <= k-1; i++)
        {
            tmpb(qtags(i)) = false;
            ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
            ap::vsub(&tmpx(0), 1, &qx(i, 0), 1, ap::vlen(0,nx-1));
            r = ap::maxreal(r, vnorm(tmpx, nx, normtype));
        }
        for(i = 0; i <= n-1; i++)
        {
            if( tmpb(i) )
            {
                ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
                ap::vsub(&tmpx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
                kdterrors = kdterrors||ap::fp_less(vnorm(tmpx, nx, normtype),r*(1-errtol)/(1+eps));
            }
        }
        for(i = 0; i <= k-2; i++)
        {
            kdterrors = kdterrors||ap::fp_greater(qr(i),qr(i+1));
        }
        for(i = 0; i <= k-1; i++)
        {
            ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
            ap::vsub(&tmpx(0), 1, &xy(qtags(i), 0), 1, ap::vlen(0,nx-1));
            kdterrors = kdterrors||ap::fp_greater(fabs(vnorm(tmpx, nx, normtype)-qr(i)),errtol);
        }
    }
    
    //
    // test general R-NN queries  (with self-matches):
    // * compare results from different trees (must be equal) and
    //   check that correct (value,tag) pairs are returned
    // * test results from XT tree - let R be radius of query result.
    //   then all points not in result must be not closer than R.
    //
    for(q = 1; q <= qcount; q++)
    {
        
        //
        // Select R
        //
        if( ap::fp_greater(ap::randomreal(),0.3) )
        {
            r = ap::maxreal(ap::randomreal(), ap::machineepsilon);
        }
        else
        {
            r = ap::machineepsilon;
        }
        
        //
        // Select point (either one of the points, or random)
        //
        if( ap::fp_greater(ap::randomreal(),0.5) )
        {
            i = ap::randominteger(n);
            ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
        }
        else
        {
            for(i = 0; i <= nx-1; i++)
            {
                ptx(i) = 2*ap::randomreal()-1;
            }
        }
        
        //
        // Test:
        // * consistency of results from different queries
        // * points in query are IN the R-sphere (or at the boundary),
        //   and points not in query are outside of the R-sphere (or at the boundary)
        // * distances are correct and are ordered
        //
        kx = kdtreequeryrnn(treex, ptx, r, true);
        kxy = kdtreequeryrnn(treexy, ptx, r, true);
        kt = kdtreequeryrnn(treext, ptx, r, true);
        if( kxy!=kx||kt!=kx )
        {
            kdterrors = true;
            return;
        }
        kx = 0;
        kxy = 0;
        kt = 0;
        kdtreequeryresultsx(treex, qx, kx);
        kdtreequeryresultsxy(treexy, qxy, kxy);
        kdtreequeryresultstags(treext, qtags, kt);
        kdtreequeryresultsdistances(treext, qr, kr);
        if( kxy!=kx||kt!=kx||kr!=kx )
        {
            kdterrors = true;
            return;
        }
        kdterrors = kdterrors||kdtresultsdifferent(xy, n, qx, qxy, qtags, kx, nx, ny);
        for(i = 0; i <= n-1; i++)
        {
            tmpb(i) = true;
        }
        for(i = 0; i <= kx-1; i++)
        {
            tmpb(qtags(i)) = false;
        }
        for(i = 0; i <= n-1; i++)
        {
            ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
            ap::vsub(&tmpx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
            if( tmpb(i) )
            {
                kdterrors = kdterrors||ap::fp_less(vnorm(tmpx, nx, normtype),r*(1-errtol));
            }
            else
            {
                kdterrors = kdterrors||ap::fp_greater(vnorm(tmpx, nx, normtype),r*(1+errtol));
            }
        }
        for(i = 0; i <= kx-2; i++)
        {
            kdterrors = kdterrors||ap::fp_greater(qr(i),qr(i+1));
        }
    }
    
    //
    // Test self-matching:
    // * self-match - nearest neighbor of each point in XY is the point itself
    // * no self-match - nearest neighbor is NOT the point itself
    //
    if( n>1 )
    {
        
        //
        // test for N=1 have non-general form, but it is not really needed
        //
        for(task = 0; task <= 1; task++)
        {
            for(i = 0; i <= n-1; i++)
            {
                ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
                kx = kdtreequeryknn(treex, ptx, 1, task==0);
                kdtreequeryresultsx(treex, qx, kx);
                if( kx!=1 )
                {
                    kdterrors = true;
                    return;
                }
                isequal = true;
                for(j = 0; j <= nx-1; j++)
                {
                    isequal = isequal&&ap::fp_eq(qx(0,j),ptx(j));
                }
                if( task==0 )
                {
                    kdterrors = kdterrors||!isequal;
                }
                else
                {
                    kdterrors = kdterrors||isequal;
                }
            }
        }
    }
}
Пример #21
0
static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, 
						   KX_Obstacles& obstacles,  float levelHeight, const float vmax,
						   const float* spos, const float cs, const int nspos, float* res, 						   
						   float maxToi, float velWeight, float curVelWeight, float sideWeight,
						   float toiWeight)
{
	vset(res, 0,0);

	const float ivmax = 1.0f / vmax;

	float adir[2], adist;
	vcpy(adir, activeObst->pvel);
	if (vlen(adir) > 0.01f)
		vnorm(adir);
	else
		vset(adir,0,0);
	float activeObstPos[2];
	vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y()); 
	adist = vdot(adir, activeObstPos);

	float minPenalty = FLT_MAX;

	for (int n = 0; n < nspos; ++n)
	{
		float vcand[2];
		vcpy(vcand, &spos[n*2]);		

		// Find min time of impact and exit amongst all obstacles.
		float tmin = maxToi;
		float side = 0;
		int nside = 0;

		for (int i = 0; i < obstacles.size(); ++i)
		{
			KX_Obstacle* ob = obstacles[i];
			bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
			if (!res)
				continue;
			float htmin, htmax;

			if (ob->m_shape==KX_OBSTACLE_CIRCLE)
			{
				float vab[2];

				// Moving, use RVO
				vscale(vab, vcand, 2);
				vsub(vab, vab, activeObst->vel);
				vsub(vab, vab, ob->vel);

				// Side
				// NOTE: dp, and dv are constant over the whole calculation,
				// they can be precomputed per object. 
				const float* pa = activeObstPos;
				float pb[2];
				vset(pb, ob->m_pos.x(), ob->m_pos.y());

				const float orig[2] = {0,0};
				float dp[2],dv[2],np[2];
				vsub(dp,pb,pa);
				vnorm(dp);
				vsub(dv,ob->dvel, activeObst->dvel);

				const float a = triarea(orig, dp,dv);
				if (a < 0.01f)
				{
					np[0] = -dp[1];
					np[1] = dp[0];
				}
				else
				{
					np[0] = dp[1];
					np[1] = -dp[0];
				}

				side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f);
				nside++;

				if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad, 
					htmin, htmax))
					continue;

				// Handle overlapping obstacles.
				if (htmin < 0.0f && htmax > 0.0f)
				{
					// Avoid more when overlapped.
					htmin = -htmin * 0.5f;
				}
			}
			else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
			{
				MT_Point3 p1 = ob->m_pos;
				MT_Point3 p2 = ob->m_pos2;
				//apply world transform
				if (ob->m_type == KX_OBSTACLE_NAV_MESH)
				{
					KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
					p1 = navmeshobj->TransformToWorldCoords(p1);
					p2 = navmeshobj->TransformToWorldCoords(p2);
				}
				float p[2], q[2];
				vset(p, p1.x(), p1.y());
				vset(q, p2.x(), p2.y());

				// NOTE: the segments are assumed to come from a navmesh which is shrunken by
				// the agent radius, hence the use of really small radius.
				// This can be handle more efficiently by using seg-seg test instead.
				// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
				const float r = 0.01f; // agent->rad
				if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad))
				{
					float sdir[2], snorm[2];
					vsub(sdir, q, p);
					snorm[0] = sdir[1];
					snorm[1] = -sdir[0];
					// If the velocity is pointing towards the segment, no collision.
					if (vdot(snorm, vcand) < 0.0f)
						continue;
					// Else immediate collision.
					htmin = 0.0f;
					htmax = 10.0f;
				}
				else
				{
					if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
						continue;
				}

				// Avoid less when facing walls.
				htmin *= 2.0f;
			}

			if (htmin >= 0.0f)
			{
				// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
				if (htmin < tmin)
					tmin = htmin;
			}
		}

		// Normalize side bias, to prevent it dominating too much.
		if (nside)
			side /= nside;

		const float vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
		const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
		const float spen = sideWeight * side;
		const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));

		const float penalty = vpen + vcpen + spen + tpen;

		if (penalty < minPenalty)
		{
			minPenalty = penalty;
			vcpy(res, vcand);
		}
	}
}
Пример #22
0
Symbolhandle    rangen(Symbolhandle list)
{
	Symbolhandle    result = (Symbolhandle) 0, symhN, symhParam1, symhParam2;
	long            sampleSize, lengthParam1, lengthParam2;
	long            op, verbose = 1;
	long            nargs = NARGS(list), margs;
	char           *what1, *what2;
	char           *badarg = "argument %ld (%s) to %s()";
	char           *badParam =
	  "ERROR: argument %ld to %s() (%s) not REAL scalar or vector";
	char           *hasMissing =
	  "ERROR: argument %ld to %s() (%s) has 1 or more MISSING values";
	char           *badLength =
	  "ERROR: length of vector argument %ld to %s() (%s) differs from sample size";
	WHERE("rangen");
	
	/* generate a list of random numbers */
	if (strcmp(FUNCNAME, "rnorm") == 0)
	{
		op = IRNORM;
	}
	else if (strcmp(FUNCNAME, "rpoi") == 0)
	{
		op = IRPOI;
		what1 = "mean";
	}
	else if (strcmp(FUNCNAME, "rbin") == 0)
	{
		op = IRBINOM;
		what1 = "n";
		what2 = "p";
	}
	else
	{
		op = IRUNI;
	}
	margs = (op == IRBINOM) ? 3 : ((op == IRPOI) ? 2 : 1);
	
	OUTSTR[0] = '\0';
	
	if (nargs != margs)
	{
		badNargs(FUNCNAME, margs);
		goto errorExit;
	}
	
	symhN = COMPVALUE(list,0);

	if (!isInteger(symhN, POSITIVEVALUE))
	{
		char    outstr[30];
		
		sprintf(outstr, badarg, 1L, "sample size", FUNCNAME);
		notPositiveInteger(outstr);
		goto errorExit;
	} /*if (!isInteger(symhN, POSITIVEVALUE))*/

	sampleSize = (long) DATAVALUE(symhN,0);

	if (margs > 1)
	{
		symhParam1 = COMPVALUE(list, 1);
		if (!argOK(symhParam1, 0, 2))
		{
			goto errorExit;
		}
		lengthParam1 = symbolSize(symhParam1);
		if (TYPE(symhParam1) != REAL || !isVector(symhParam1))
		{
			sprintf(OUTSTR, badParam, 2L, what1, FUNCNAME);
		}
		else if (anyMissing(symhParam1))
		{
			sprintf(OUTSTR, hasMissing, 2L, what1, FUNCNAME);
		}
		else if (lengthParam1 > 1 && lengthParam1 != sampleSize)
		{
			sprintf(OUTSTR, badLength, 2L, what1, FUNCNAME);
		}
		else if (margs > 2)
		{
			symhParam2 = COMPVALUE(list, 2);
			if (!argOK(symhParam2, 0, 3))
			{
				goto errorExit;
			}

			lengthParam2 = symbolSize(symhParam2);
			if (TYPE(symhParam2) != REAL || !isVector(symhParam2))
			{
				sprintf(OUTSTR, badParam, 3L, what2, FUNCNAME);
			}
			else if (anyMissing(symhParam2))
			{
				sprintf(OUTSTR, hasMissing, 3L, what2, FUNCNAME);
			}
			else if (lengthParam2 > 1 && lengthParam2 != sampleSize)
			{
				sprintf(OUTSTR, badLength, 3L, what2, FUNCNAME);
			}
		}

		if (*OUTSTR)
		{
			goto errorExit;
		}
		
		if (op == IRPOI)
		{
			/*rpoi(lambda)*/
			if (doubleMin(DATAPTR(symhParam1), lengthParam1) < 0.0)
			{
				sprintf(OUTSTR,
						"ERROR: argument 2 (%s) to %s() has negative element",
						what1, FUNCNAME);
			}
		}
		else if (op == IRBINOM)
		{
			/*rbinom(samplesize,n,p)*/
			long        i;
			double     *n = DATAPTR(symhParam1);

			for (i = 0; i < lengthParam1; i++)
			{
				if (n[i] < 1 || n[i] != floor(n[i]))
				{
					sprintf(OUTSTR,
							"ERROR: not all elements of argument 2 (%s) to %s() are positive integers",
							what1, FUNCNAME);
					goto errorExit;
				}
			} /*for (i = 0; i < lengthParam1; i++)*/

			if (doubleMin(DATAPTR(symhParam2), lengthParam2) < 0.0 ||
					 doubleMax(DATAPTR(symhParam2), lengthParam2) > 1.0)
			{
				sprintf(OUTSTR,
						"ERROR: argument 3 (%s) to %s() has value < 0 or > 1",
						what2, FUNCNAME);
			}
		} /*else if (op == IRBINOM)*/

		if (*OUTSTR)
		{
			goto errorExit;
		}
	} /*if (margs > 1)*/

	if (Rands1 == 0 && Rands2 == 0)
	{
		randomSeed(verbose);
	} /*if (Rands1 == 0 && Rands2 == 0)*/

	result = RInstall(SCRATCH, sampleSize);
	if (result != (Symbolhandle) 0)
	{
		switch (op)
		{
		  case IRUNI:
			vuni(sampleSize, DATAPTR(result));
			break;
		  case IRNORM:
			vnorm(sampleSize, DATAPTR(result));
			break;
		  case IRPOI:
			vpoi(sampleSize, DATAPTR(symhParam1), lengthParam1,
				 DATAPTR(result));
			break;
		  case IRBINOM:
			vbinom(sampleSize, DATAPTR(symhParam1), lengthParam1,
				   DATAPTR(symhParam2), lengthParam2, DATAPTR(result));
			break;
		} /*switch (op)*/
	} /*if (result != (Symbolhandle) 0)*/

	return (result);

  errorExit:
	putErrorOUTSTR();

	return (0);
	
} /*rangen()*/
Пример #23
0
void
define_plane (double x1, double y1, double z1,
	      double x2, double y2, double z2,
	      double x3, double y3, double z3,
	      double normx, double normy, double normz)
{
	struct groundtri *gt;
	struct vect v1, v2;
	
	gt = xcalloc (1, sizeof *gt);
	
	if (first_groundtri == NULL) {
		first_groundtri = gt;
	} else {
		last_groundtri->next = gt;
	}

	last_groundtri = gt;
	
	gt->pl.a = normx;
	gt->pl.b = normy;
	gt->pl.c = normz;
	gt->pl.d = -(gt->pl.a * x1 + gt->pl.b * y1 + gt->pl.c * z1);

	gt->normv.x = normx;
	gt->normv.y = normy;
	gt->normv.z = normz;

	vnorm (&gt->normv, &gt->normv);

	gt->p1.x = x1;
	gt->p1.y = y1;
	gt->p1.z = z1;
	gt->p2.x = x2;
	gt->p2.y = y2;
	gt->p2.z = z2;
	gt->p3.x = x3;
	gt->p3.y = y3;
	gt->p3.z = z3;

	gt->pl.middle.x = (x1 + x2 + x3) / 3;
	gt->pl.middle.y = (y1 + y2 + y3) / 3;
	gt->pl.middle.z = (z1 + z2 + z3) / 3;

	if (fabs (gt->pl.c) < 1e-6) {
		gt->downhill.x = 0;
		gt->downhill.y = 0;
		gt->downhill.z = -1;
	} else if (hypot (gt->pl.a, gt->pl.b) < 1e-6) {
		gt->downhill.x = 1;
		gt->downhill.y = 0;
		gt->downhill.z = 0;
	} else {
		gt->downhill.x = gt->pl.a;
		gt->downhill.y = gt->pl.b;
		gt->downhill.z = (-gt->pl.a * gt->pl.a - gt->pl.b * gt->pl.b - gt->pl.d)
			/ gt->pl.c;
	}

	vnorm (&gt->downhill, &gt->downhill);
	
	vscal (&v1, &gt->downhill, -1);
	if (v1.x == 0 && v1.y == 0) {
		v2.x = 1;
		v2.y = 1;
		v2.z = 0;
	} else {
		v2.x = v1.x;
		v2.y = v1.y;
		v2.z = 0;
	}

	gt->theta = acos (vdot (&v1, &v2));

	gt->gndnum = gndcounter;
	gndcounter++;
}
Пример #24
0
void
DoFlares(GLfloat from[3], GLfloat at[3], GLfloat light[3], GLfloat near_clip)
{
  GLfloat view_dir[3], tmp[3], light_dir[3], position[3], dx[3], dy[3],
    center[3], axis[3], sx[3], sy[3], dot, global_scale = 1.5;
  GLuint bound_to = 0;
  int i;

  /* view_dir = normalize(at-from) */
  vdiff(view_dir, at, from);
  vnorm(view_dir);

  /* center = from + near_clip * view_dir */
  vscale(tmp, view_dir, near_clip);
  vadd(center, from, tmp);

  /* light_dir = normalize(light-from) */
  vdiff(light_dir, light, from);
  vnorm(light_dir);

  /* light = from + dot(light,view_dir)*near_clip*light_dir */
  dot = vdot(light_dir, view_dir);
  vscale(tmp, light_dir, near_clip / dot);
  vadd(light, from, light_dir);

  /* axis = light - center */
  vdiff(axis, light, center);
  vcopy(dx, axis);

  /* dx = normalize(axis) */
  vnorm(dx);

  /* dy = cross(dx,view_dir) */
  vcross(dy, dx, view_dir);

  glDisable(GL_DEPTH_TEST);
  glDisable(GL_DITHER);
  glEnable(GL_BLEND);
  glBlendFunc(GL_ONE, GL_ONE);

  for (i = 0; i < num_flares; i++) {
    vscale(sx, dx, flare[i].scale * global_scale);
    vscale(sy, dy, flare[i].scale * global_scale);

    glColor3fv(flare[i].color);
    /* Note logic below to eliminate duplicate texture binds. */
    if (flare[i].type < 0) {
      if (bound_to)
	glEnd();
      glBindTexture(GL_TEXTURE_2D, shineTex[shine_tic]);
      bound_to = shineTex[shine_tic];
      shine_tic = (shine_tic + 1) % 10;
      glBegin(GL_QUADS);
    } else {
      if (bound_to != flareTex[flare[i].type]) {
	glEnd();
        glBindTexture(GL_TEXTURE_2D, flareTex[flare[i].type]);
        bound_to = flareTex[flare[i].type];
	glBegin(GL_QUADS);
      }
    }

    /* position = center + flare[i].loc * axis */
    vscale(tmp, axis, flare[i].loc);
    vadd(position, center, tmp);

    glTexCoord2f(0.0, 0.0);
    vadd(tmp, position, sx);
    vadd(tmp, tmp, sy);
    glVertex3fv(tmp);

    glTexCoord2f(1.0, 0.0);
    vdiff(tmp, position, sx);
    vadd(tmp, tmp, sy);
    glVertex3fv(tmp);

    glTexCoord2f(1.0, 1.0);
    vdiff(tmp, position, sx);
    vdiff(tmp, tmp, sy);
    glVertex3fv(tmp);

    glTexCoord2f(0.0, 1.0);
    vadd(tmp, position, sx);
    vdiff(tmp, tmp, sy);
    glVertex3fv(tmp);
  }
  glEnd();
}