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