static void vehicleReset(Chassis* chassis, float mass, const vec3* offset)
{
Chassis* c = chassis;
//================
// Reset chassis
//================
c->mass = mass;
c->inertia = 2.f/5.f * SQR(1.f) * c->mass;
matrixIdent(&c->pose);
c->pose.v[3].v3 = *offset;
veczero(&c->vel);
veczero(&c->angVel);
c->steer = 1.0f;
//==================
// Reset suspension
//==================
vec3 offset0 = {+1.f, +1.4f+0.6f, 0.f};
vec3 offset1 = {-1.f, +1.4f+0.6f, 0.f};
vec3 offset2 = {+1.f, -1.4f+0.6f, 0.f};
vec3 offset3 = {-1.f, -1.4f+0.6f, 0.f};
suspensionReset(c->suspension[0], &offset0);
suspensionReset(c->suspension[1], &offset1);
suspensionReset(c->suspension[2], &offset2);
suspensionReset(c->suspension[3], &offset3);
Wheel* w0 = c->suspension[0]->wheel;
Wheel* w1 = c->suspension[1]->wheel;
Wheel* w2 = c->suspension[2]->wheel;
Wheel* w3 = c->suspension[3]->wheel;
w0->maxSteer = DEG2RAD(35.f);
w1->maxSteer = DEG2RAD(35.f);
w2->maxSteer = DEG2RAD(0.f);
w3->maxSteer = DEG2RAD(0.f);
w0->driven = true;
w1->driven = true;
w2->driven = false;
w3->driven = false;
// Set the correct wheel position
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
vec3mtx33mulvec3(&s->worldOffset, &c->pose, &s->offset);
vec3mtx43mulvec3(&s->worldDefaultPos, &c->pose, &s->offset);
w->pos = s->worldDefaultPos;
}
}
void crf1dc_partial_beta_score(crf1d_context_t* ctx, int *mask)
{
int i, j, t;
int *curr_mask, *next_mask;
floatval_t *cur = NULL;
floatval_t *row = ctx->row;
const floatval_t *next = NULL, *state = NULL, *trans = NULL;
const int T = ctx->num_items;
const int L = ctx->num_labels;
const floatval_t *scale = &ctx->partial_scale_factor[T-1];
/* Compute the beta scores at (T-1, *). */
cur = PARTIAL_BETA_SCORE(ctx, T-1);
veczero(cur, L);
curr_mask = &mask[(T-1)*L];
for (i = 0; i < L; ++ i) {
if (curr_mask[i]) {
cur[i] = *scale;
}
}
--scale;
/* Compute the beta scores at (t, *). */
for (t = T-2;0 <= t;--t) {
cur = PARTIAL_BETA_SCORE(ctx, t);
next = PARTIAL_BETA_SCORE(ctx, t+1);
state = EXP_STATE_SCORE(ctx, t+1);
curr_mask = &mask[t * L];
next_mask = &mask[(t+1) * L];
veccopy(row, next, L);
veczero(cur, L);
for (i = 0; i < L; ++ i) {
if (next_mask[i]) {
row[i] *= state[i];
}
}
for (j = 0; j < L; ++ j) {
if (curr_mask[j]) {
trans = EXP_TRANS_SCORE(ctx, j);
for (i = 0; i < L; ++ i) {
if (next_mask[i]) {
cur[j] += trans[i] * row[i];
}
}
}
}
vecscale(cur, *scale, L);
--scale;
}
}
static void suspensionReset(Suspension* s, const vec3* offset)
{
Wheel* w = s->wheel;
s->offset = *offset;
vec3* chassisPos = &s->chassis->pose.v[3].v3;
veczero(&s->wheel->vel);
w->angSpeed = 0.f;
w->radius = 0.3f;
float smallestMass = s->chassis->mass * 0.05f;
if (w->mass < smallestMass) w->mass = smallestMass;
w->inertia = 2.f/5.f*w->radius*w->radius*w->mass;
w->invInertia = 1.f/w->inertia;
}
void crf1dc_alpha_score(crf1d_context_t* ctx)
{
int i, t;
floatval_t sum, *cur = NULL;
floatval_t *scale = &ctx->scale_factor[0];
const floatval_t *prev = NULL, *trans = NULL, *state = NULL;
const int T = ctx->num_items;
const int L = ctx->num_labels;
/* Compute the alpha scores on nodes (0, *).
alpha[0][j] = state[0][j]
*/
cur = ALPHA_SCORE(ctx, 0);
state = EXP_STATE_SCORE(ctx, 0);
veccopy(cur, state, L);
sum = vecsum(cur, L);
*scale = (sum != 0.) ? 1. / sum : 1.;
vecscale(cur, *scale, L);
++scale;
/* Compute the alpha scores on nodes (t, *).
alpha[t][j] = state[t][j] * \sum_{i} alpha[t-1][i] * trans[i][j]
*/
for (t = 1;t < T;++t) {
prev = ALPHA_SCORE(ctx, t-1);
cur = ALPHA_SCORE(ctx, t);
state = EXP_STATE_SCORE(ctx, t);
veczero(cur, L);
for (i = 0;i < L;++i) {
trans = EXP_TRANS_SCORE(ctx, i);
vecaadd(cur, prev[i], trans, L);
}
vecmul(cur, state, L);
sum = vecsum(cur, L);
*scale = (sum != 0.) ? 1. / sum : 1.;
vecscale(cur, *scale, L);
++scale;
}
/* Compute the logarithm of the normalization factor here.
norm = 1. / (C[0] * C[1] ... * C[T-1])
log(norm) = - \sum_{t = 0}^{T-1} log(C[t]).
*/
ctx->log_norm = -vecsumlog(ctx->scale_factor, T);
}
void crf1dc_reset(crf1d_context_t* ctx, int flag)
{
const int T = ctx->num_items;
const int L = ctx->num_labels;
if (flag & RF_STATE) {
veczero(ctx->state, T*L);
}
if (flag & RF_TRANS) {
veczero(ctx->trans, L*L);
}
if (ctx->flag & CTXF_MARGINALS) {
veczero(ctx->mexp_state, T*L);
veczero(ctx->mexp_trans, L*L);
veczero(ctx->partial_mexp_state, T*L);
veczero(ctx->partial_mexp_trans, L*L);
ctx->log_norm = 0;
ctx->partial_log_norm = 0;
}
}
void crf1dc_marginal_without_beta(crf1d_context_t* ctx)
{
int i, j, t;
floatval_t *prob = NULL;
floatval_t *row = ctx->row;
const floatval_t *fwd = NULL;
const int T = ctx->num_items;
const int L = ctx->num_labels;
/*
Compute marginal probabilities of states at T-1
p(T-1,j) = fwd'[T-1][j]
*/
fwd = ALPHA_SCORE(ctx, T-1);
prob = STATE_MEXP(ctx, T-1);
veccopy(prob, fwd, L);
/*
Repeat the following computation for t = T-1,T-2, ..., 1.
1) Compute p(t-1,i,t,j) using p(t,j)
2) Compute p(t,i) using p(t-1,i,t,j)
*/
for (t = T-1;0 < t;--t) {
fwd = ALPHA_SCORE(ctx, t-1);
prob = STATE_MEXP(ctx, t);
veczero(ctx->adj, L*L);
veczero(row, L);
/*
Compute adj[i][j] and row[j].
adj[i][j] = fwd'[t-1][i] * edge[i][j]
row[j] = \sum_{i} adj[i][j]
*/
for (i = 0;i < L;++i) {
floatval_t *adj = ADJACENCY(ctx, i);
floatval_t *edge = EXP_TRANS_SCORE(ctx, i);
vecaadd(adj, fwd[i], edge, L);
vecadd(row, adj, L);
}
/*
Find z such that z * \sum_{i] adj[i][j] = p(t,j).
Thus, z = p(t,j) / row[j]; we overwrite row with z.
*/
vecinv(row, L);
vecmul(row, prob, L);
/*
Apply the partition factor z (row[j]) to adj[i][j].
*/
for (i = 0;i < L;++i) {
floatval_t *adj = ADJACENCY(ctx, i);
vecmul(adj, row, L);
}
/*
Now that adj[i][j] presents p(t-1,i,t,j),
accumulate model expectations of transitions.
*/
for (i = 0;i < L;++i) {
floatval_t *adj = ADJACENCY(ctx, i);
floatval_t *prob = TRANS_MEXP(ctx, i);
vecadd(prob, adj, L);
}
/*
Compute the marginal probability of states at t-1.
p(t-1,i) = \sum_{j} p(t-1,i,t,j)
*/
prob = STATE_MEXP(ctx, t-1);
for (i = 0;i < L;++i) {
floatval_t *adj = ADJACENCY(ctx, i);
prob[i] = vecsum(adj, L);
}
}
}
void crf1dc_partial_alpha_score(crf1d_context_t* ctx, int *mask)
{
int i, j, t;
int *prev_mask, *curr_mask;
floatval_t sum, *cur = NULL;
floatval_t *scale = &ctx->partial_scale_factor[0];
const floatval_t *prev = NULL, *trans = NULL, *state = NULL;
const int T = ctx->num_items;
const int L = ctx->num_labels;
/* Compute the alpha scores on nodes (0, *).
alpha[0][j] = state[0][j]
*/
cur = PARTIAL_ALPHA_SCORE(ctx, 0);
veczero(cur, L);
state = EXP_STATE_SCORE(ctx, 0);
curr_mask = &mask[0];
for (i = 0; i < L; ++ i) {
if (curr_mask[i]) {
cur[i] = state[i];
}
}
sum = vecsum(cur, L);
/* scale is a temporary structure */
*scale = (sum != 0.) ? 1. / sum : 1.;
vecscale(cur, *scale, L);
++scale;
/* Compute the alpha scores on nodes (t, *).
alpha[t][j] = state[t][j] * \sum_{i} alpha[t-1][i] * trans[i][j]
*/
for (t = 1;t < T;++t) {
prev = PARTIAL_ALPHA_SCORE(ctx, t-1);
cur = PARTIAL_ALPHA_SCORE(ctx, t);
state = EXP_STATE_SCORE(ctx, t);
prev_mask = &mask[(t-1) * L];
curr_mask = &mask[t * L];
veczero(cur, L);
for (i = 0; i < L; ++ i) {
if (prev_mask[i]) {
trans = EXP_TRANS_SCORE(ctx, i);
for (j = 0; j < L; ++ j) {
if (curr_mask[j]) {
cur[j] += prev[i] * trans[j];
}
}
}
}
for (j = 0; j < L; ++ j) {
if (curr_mask[j]) {
cur[j] *= state[j];
}
}
sum = vecsum(cur, L);
*scale = (sum != 0.) ? 1. / sum : 1.;
vecscale(cur, *scale, L);
++scale;
}
/* Compute the logarithm of the normalization factor here.
norm = 1. / (C[0] * C[1] ... * C[T-1])
log(norm) = - \sum_{t = 0}^{T-1} log(C[t]).
*/
ctx->partial_log_norm = -vecsumlog(ctx->partial_scale_factor, T);
}
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;
}
