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);
}
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);
}
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;
}
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);
}
}