Example #1
0
static void doCorrection(Chassis* c, const vec3* worldOffset, const vec3* axis, float requiredVelocityChange)
{
	float denom = computeDenominator(1.f/c->mass, 1.f/c->inertia, worldOffset, axis);
	float correction = requiredVelocityChange / denom;
	vec3 impulse;
	vecscale(&impulse, axis, correction);
	addImpulseAtOffset(&c->vel, &c->angVel, 1.f/c->mass, 1.f/c->inertia, worldOffset, &impulse);
}
Example #2
0
void Cosine::computeScore(size_t docId){
	// TODO given a doc Id, compute the score of this doc.
	float cossim=0;

	// get forward index and make it unique
	set<size_t> frwdIndx =this->frwdReader->getUniqueTermId(docId);

	// get term frequency
	//	!!! need an interface..
	map<size_t, size_t> docTermFreq;
	set<size_t>::iterator itDocTermFreq = frwdIndx.begin();
	size_t count = 0;
	for (; itDocTermFreq!=frwdIndx.end(); ++itDocTermFreq){
		docTermFreq[*itDocTermFreq] = this->invtReader.getTF(this->queryTerm.at(count), docId);
		//docTermFreq[*itDocTermFreq] = 1;
		++count;
	}

	// intersection of two sets
	vector<size_t> interTermId(min(this->uniQueryTermId.size(), frwdIndx.size()));
	vector<size_t>::iterator itInterset;
	itInterset = set_intersection (this->uniQueryTermId.begin(), this->uniQueryTermId.end(), frwdIndx.begin(), frwdIndx.end(), interTermId.begin());
	interTermId.erase(itInterset, interTermId.end());

	// compute un-normalized cosine similarity
	itInterset = interTermId.begin();
	for(; itInterset!=interTermId.end(); ++itInterset){
		cossim += (docTermFreq[*itInterset] * this->uniQueryTermFreq[*itInterset]);
	}

	// compute the denominators for current document
	float docDenominator = computeDenominator(frwdIndx, docTermFreq);

	// push back the scores into member variant
	cout << "iterset num: " << interTermId.size() <<"  || q term num: " << uniQueryTermId.size() << " || uniqdoctermId: " << frwdIndx.size() << endl;
	cout << "unnorm cossim: " << cossim;
	cossim = cossim / docDenominator /this->queryDenominator;
	cout << " |||| docDenom: " << docDenominator << " |||| queryDenom: "<<this->queryDenominator<<endl;
	this->score.push_back(cossim);
}
Example #3
0
int main (int argc, char **argv)
{
#if 0
	
	const int N = 4;
	float y[N] = {-0.653828, -0.653828, 0.753333, 0.753333};
	float k[N];

	float l[2] = {0.f, 0.f};
	float kdl[2] = {0.f, 0.f};
	float n[2] = {0.f, 0.f};

	for (int i=0; i<N; i++)
	{
		if (y[i] > 0.f)
		{
			l[1] += y[i];
			n[1] += 1.f;
		}
		else
		{
			l[0] -= y[i];
			n[0] += 1.f;
		}
	}

	kdl[1] = l[1] / (n[1]*l[0] + n[0]*l[1]);
	kdl[0] = l[0] / (n[1]*l[0] + n[0]*l[1]);

	for (int i=0; i<2; i++)
	{
		printf("[%d] l = %f kdl = %f\n", i, l[i], kdl[i]);
	}

	for (int i=0; i<N; i++)
	{
		k[i] = y[i] > 0.f ? kdl[1] : kdl[0];
	}


	float force = 0.f;
	float torque = 0.f;
	for (int i=0; i<N; i++)
	{
		force += k[i]; 
		torque += y[i] * k[i];
	}

	printf("force = %f\n", force);
	printf("torque = %f\n", torque);

	printf("kdl[0]/kdl[1] = %f\n", kdl[0]/kdl[1]);



	return 0;

#endif

#if 0
	vec3 r = {1.f, -0.3f, 0.f};
	vec3 fground = {0.f, 1.f, 0.f};
	vec3 fcent = {-1.f, 0.f, 0.f};

	vec3 tground;
	vec3 tcent;
	veccross(&tground, &r, &fground);
	veccross(&tcent, &r, &fcent);

	printf("tground = %f\n", tground.z);
	printf("tcent = %f\n", tcent.z);

	return 0;
	float axleFriction = 200.1f;
	float mass = 10.f;
	float invMass = 1.f/mass;
	float v = 10.0f;
	float dt = 0.01;

	for (int r=0; r<100; r++)
	{
		float force = -axleFriction*sgn(v);
		v = v + force*invMass * dt;
		printf("v = %f\n", v);
	}

	return 0;
#endif

#if 0
	float mass = 10.f;
	float wheelmass = 1.0f;
	float radius = 0.1f;
	float wheelInertia = 2.f/5.f*radius*radius*wheelmass;
	float vel = 0.f;
	float wheelVel= 0.f;

	float dt = 0.01f;
	float torque = 1000.f;
	float angSpeed = -dt*torque*radius/wheelInertia; 

	float momentum = angSpeed*wheelInertia;
	printf("momentum put in = %f \n", momentum);

	for (int repeat = 0; repeat<10; repeat++)
	{
		{
			float contactSpeed = radius * angSpeed + wheelVel;
			float error = contactSpeed;
			float denom = 1.f/wheelmass + radius*radius/wheelInertia;
			float impulse = error / denom;

			// Add impulse to the wheel
			wheelVel = wheelVel - impulse/wheelmass;
			angSpeed = angSpeed - radius*impulse/wheelInertia;
		}

		// Axis error
		{
			float error = wheelVel - vel;
			float denom = 1.f/wheelmass + 1.f/mass;
			float impulse = error/denom;
			wheelVel = wheelVel - impulse/wheelmass;
			vel = vel + impulse/mass;
		}
	}
	
	printf("momentum c = %f\n", vel*mass);
	printf("momentum w = %f\n", vel*wheelmass);
	printf("momentum aw = %f\n", angSpeed*wheelInertia);
	printf("total momentum = %f\n", vel*(mass+wheelmass) + angSpeed*wheelInertia);
	printf("chassis = %f, wheel = %f\n", vel, wheelVel);

	return 0;
#endif

#if 0
	float mass = 10.f;
	float inertia = mass * 0.4f;
	vec3 wheelOffset = {0.f, 1.5f, -0.2f};
	float wheelmass = 1.0f;
	float radius = 0.1f;
	float wheelInertia = 2.f/5.f*radius*radius*wheelmass;
	vec3 vel = {0.f, 0.f, 0.f};
	vec3 w = {0.f, 0.f, 0.f};
	float wheelVel= 0.f;

	float dt = 0.01f;
	float torque = 1000.f;
	float angSpeed = -dt*torque*radius/wheelInertia; 

	for (int repeat = 0; repeat<100; repeat++)
	{
		{
			float contactSpeed = radius * angSpeed + wheelVel;
			float error = contactSpeed;
			float denom = 1.f/wheelmass + radius*radius/wheelInertia;
			float impulse = error / denom;

			// Add impulse to the wheel
			wheelVel = wheelVel - impulse/wheelmass;
			angSpeed = angSpeed - radius*impulse/wheelInertia;
		}

		// Axis error
		{
			// float axleVel = vel;
			vec3 cross;
			veccross(&cross, &w, &wheelOffset);
			float axleVel = vel.y + cross.y;

			vec3 pulldir = {0.f, 1.f, 0.f};
			float error = wheelVel - axleVel;
			if (error < 0.000001f) break;
			float denom = 1.f/wheelmass + computeDenominator(1.f/mass, 1.f/inertia, &wheelOffset, &pulldir);
			float impulse = error/denom;
			wheelVel = wheelVel - impulse/wheelmass;
			//vel = vel + impulse/mass;
			{
				vecscale(&pulldir, &pulldir, impulse);
				addImpulseAtOffset(&vel, &w, 1.f/mass, 1.f/inertia, &wheelOffset, &pulldir);
			}
		}
	}

	printf("wheelVel = %f, vel = %f, w = %f\n", wheelVel, vel.y, w.x);
	printf("%f\n", w.x/vel.y);

	// Simple force application!
	{
		wheelVel = 0.f;
		vec3 impulse = {0.f, dt * torque / radius, 0.f};
		veczero(&vel);
		veczero(&w);
		vec3 offset = wheelOffset;
		//offset.z -= radius;
		printf("wheelVel = %f, vel = %f, w = %f\n", wheelVel, vel.y, w.x);
		addImpulseAtOffset(&vel, &w, 1.f/mass, 1.f/inertia, &offset, &impulse);
	}

	printf("wheelVel = %f, vel = %f, w = %f\n", wheelVel, vel.y, w.x);

	printf("%f\n", w.x/vel.y);

	return 0;
#endif

#if 0
	float x = 100.f;
	float friction = 20.f;
	float dt = 0.01f;
	float r = dt*friction;
	int n=0;

	while (n<1000)
	{
		//x = x - r*x/(fabsf(x)+r);
		printf("%f\n", x);
		n++;
	}


	return 0;
#endif

	timerUpdate(&g_time);
    vehicleInit();

    // GLUT Window Initialization:
    glutInit (&argc, argv);
    glutInitWindowSize (s_width, s_height);
    glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
    glutCreateWindow ("CS248 GLUT example");

    // Initialize OpenGL graphics state
    initGraphics();

    // Register callbacks:
    glutDisplayFunc (display);
    glutReshapeFunc (reshape);
    glutKeyboardFunc (keyboard);
    glutMouseFunc (mouseButton);
    glutMotionFunc (mouseMotion);
    glutIdleFunc (animateScene);

    //BuildPopupMenu ();
    //glutAttachMenu (GLUT_RIGHT_BUTTON);

    // Turn the flow of control over to GLUT
    glutMainLoop ();

    return 0;
}
Example #4
0
static void vehicleSubTick(Chassis* c, float dt)
{
	if (g_step==0) return;
	if (g_step&1) g_step = 0;

	vec3* chassisPos = &c->pose.v[3].v3;
	vec3* x = &c->pose.v[0].v3;
	vec3* y = &c->pose.v[1].v3;
	vec3* z = &c->pose.v[2].v3;

	// This bit is done by the physics engine
	if(1)
	{
		vecaddscale(chassisPos, chassisPos, &c->vel, dt);
		mtx rot;
		matrixRotateByVelocity(&rot, &c->pose, &c->angVel, dt);
		matrixCopy33(&c->pose, &rot);
	}

	// Damp
	vecscale(&c->vel, &c->vel, expf(-dt*1.f));
	vecscale(&c->angVel, &c->angVel, expf(-dt*1.f));

	if (fabsf(c->angVel.x)<0.01f) c->angVel.x = 0.f;
	if (fabsf(c->angVel.y)<0.01f) c->angVel.y = 0.f;
	if (fabsf(c->angVel.z)<0.01f) c->angVel.z = 0.f;

	ClampedImpulse frictionImpulse[numWheels][2];
	
	c->steer = g_steer;

	//g_steer *= expf(-dt*3.f);
	//g_speed *= expf(-dt*3.f);

	static float latf = 10.f;
	static float angSpeed = 0.f;

	if (g_handBrake>0.f)
	{
		g_handBrake *= expf(-4.f*dt);
		g_speed *= expf(-4.f*dt);
		if (g_handBrake < 0.1f)
		{
			g_handBrake = 0.f;
		}
	}

	// Prepare
	for (int i=0; i<numWheels; i++)
	{
		Suspension* s = c->suspension[i];
		Wheel* w = s->wheel;

		// Calculate the world position and offset of the suspension point
		vec3mtx33mulvec3(&s->worldOffset, &c->pose, &s->offset);
		vec3mtx43mulvec3(&s->worldDefaultPos, &c->pose, &s->offset);

		w->pos = s->worldDefaultPos;
		vec3 pointVel = getPointVel(c, &s->worldOffset);
		vecadd(&w->vel, &w->vel, &pointVel);
		
		float maxFriction0 = 2.0f * dt * c->mass * gravity * (1.f/(float)numWheels);
		clampedImpulseInit(&frictionImpulse[i][0], maxFriction0);

		float latfriction = 10.f;

		float newAngSpeed = vecdot(z, &c->angVel);
		float changeAngSpeed = (newAngSpeed - angSpeed)/dt;
		angSpeed = newAngSpeed;
		printf("changeAngSpeed = %f\n", changeAngSpeed);
		float speed = fabsf(vecdot(y, &c->vel));
		const float base = 0.5f;
		if (g_speed>=0 && i>=2)
		{
			latfriction = 1.f*expf(-5.f*g_handBrake) + base;

			// latfriction = 1.f*expf(-2.f*fabsf(speed*changeAngSpeed)) + base;

			// if (angSpeed*g_steer < -0.1f)
			// {
			// 	latfriction = base;
			// }

			//if (g_steer == 0.f)
			//{
			//	latf += (10.f - latf) * (1.f - exp(-0.1f*dt));
			//}
			//else
			//{
			//	latf += (0.1f - latf) * (1.f - exp(-10.f*dt));
			//}
			//latfriction = latf;
		}
		else
		{ 
			latfriction = 10.f;
		}

		float maxFriction1 = latfriction * dt * c->mass * gravity * (1.f/(float)numWheels);
		clampedImpulseInit(&frictionImpulse[i][1], maxFriction1);
				
		vecset(&s->hitNorm, 0.f, 0.f, 1.f);

		float steer = w->maxSteer*c->steer * (1.f + 0.3f*s->offset.x*c->steer);


		vecscale(&w->wheelAxis, x, cosf(steer));
		vecsubscale(&w->wheelAxis, &w->wheelAxis, y, sinf(steer));

		w->frictionDir[0];
		veccross(&w->frictionDir[0], z, &w->wheelAxis);
		w->frictionDir[1] = w->wheelAxis;

		w->angSpeed = -40.f*g_speed;
	}


	//=============
	//   VERBOSE
	//=============
	#define verbose false
	#define dump if (verbose) printf

	dump("==========================================\n");
	dump("START ITERATION\n");
	dump("==========================================\n");

	float solverERP = numIterations>1 ? 0.1f : 1.f;
	float changeSolverERP = numIterations>1 ? (1.f - solverERP)/(0.01f+ (float)(numIterations-1)) : 0.f;

	for (int repeat=0; repeat<numIterations; repeat++)
	{
		dump(" == Start Iter == \n");

		for (int i=0; i<numWheels; i++)
		{
			Suspension* s = c->suspension[i];
			Wheel* w = s->wheel;
		
			const bool axisError = true;
			const bool friction = true;
		
			// Friction
			if (friction)
			{
				vec3 lateralVel;
				vecaddscale(&lateralVel, &w->vel, &s->hitNorm, -vecdot(&s->hitNorm, &w->vel));
				vecaddscale(&lateralVel, &lateralVel, &w->frictionDir[0], +w->angSpeed * w->radius);

				{
					int dir = 0;
					float v = vecdot(&lateralVel, &w->frictionDir[dir]);
					float denom = 1.f/w->mass + w->radius*w->radius*w->invInertia;
					float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
					vec3 impulseV;
					vecscale(&impulseV, &w->frictionDir[dir], impulse);
					vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
					w->angSpeed = w->angSpeed + (impulse * w->radius * w->invInertia);
				}
				if (1)
				{
					int dir=1;
					float v = vecdot(&lateralVel, &w->frictionDir[dir]);
					float denom = 1.f/w->mass;
					float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
					vec3 impulseV;
					vecscale(&impulseV, &w->frictionDir[dir], impulse);
					vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
				}

				//dump("gound collision errorV = %f, vel of wheel after = %f\n", penetration, vecdot(&w->vel, &s->hitNorm));
			}	

			if (axisError)	// Axis Error
			{
				vec3 offset;
				vecsub(&offset, &w->pos, chassisPos);
				vec3 pointvel = getPointVel(c, &offset);
				vec3 error;
				vecsub(&error, &pointvel, &w->vel);
				vecaddscale(&error, &error, z, -vecdot(&error, z));

				vec3 norm;
				if (vecsizesq(&error)>0.001f)
				{
					dump("axis error %f\n", vecsize(&error));
					vecnormalise(&norm, &error);

					float denom = computeDenominator(1.f/c->mass, 1.f/c->inertia, &offset, &norm) + 1.f/w->mass;
					vecscale(&error, &error, -solverERP/denom);
					addImpulseAtOffset(&c->vel, &c->angVel, 1.f/c->mass, 1.f/c->inertia, &offset, &error);
					vecaddscale(&w->vel, &w->vel, &error, -solverERP/w->mass);
				}
				//dump("axis error vel of wheel after = %f, inline = %f\n", vecdot(&w->vel, &s->hitNorm), vecdot(&w->vel, &s->axis));
			}
		}
		solverERP += changeSolverERP;
	}

	for (int i=0; i<numWheels; i++)
	{
		Suspension* s = c->suspension[i];
		Wheel* w = s->wheel;
		vec3 pointVel = getPointVel(c, s);
		
		// Convert suspension wheel speed back to car space
		vecsub(&w->vel, &w->vel, &pointVel);
	}
}